ethercatcoe.c

Go to the documentation of this file.

/*
 * Simple Open EtherCAT Master Library 
 *
 * File    : ethercatcoe.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 <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/ethercatcoe.h>

typedef struct
  PACKED
  {
    ec_mbxheadert MbxHeader;
    uint16 CANOpen;
    uint8 Command;
    uint16 Index;
    uint8 SubIndex;
    union
    {
      uint8 bdata[0x200]; /* variants for easy data access */
      uint16 wdata[0x100];
      uint32 ldata[0x80];
    };
  } ec_SDOt;

  typedef struct
    PACKED
    {
      ec_mbxheadert MbxHeader;
      uint16 CANOpen;
      uint8 Opcode;
      uint8 Reserved;
      uint16 Fragments;
      union
      {
        uint8 bdata[0x200]; /* variants for easy data access */
        uint16 wdata[0x100];
        uint32 ldata[0x80];
      };
    } ec_SDOservicet;

    typedef struct
      PACKED
      {
        uint8 n;
        uint8 nu1;
        uint8 SMtype[EC_MAXSM];
      } ec_SMcommtypet;

      typedef struct
        PACKED
        {
          uint8 n;
          uint8 nu1;
          uint16 index[256];
        } ec_PDOassignt;

        typedef struct
          PACKED
          {
            uint8 n;
            uint8 nu1;
            uint32 PDO[256];
          } ec_PDOdesct;

          static ec_SMcommtypet ec_SMcommtype;
          static ec_PDOassignt ec_PDOassign;
          static ec_PDOdesct ec_PDOdesc;

          void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
          {
            ec_errort Ec;

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

          static void ec_SDOinfoerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
          {
            ec_errort Ec;

            Ec.Slave = Slave;
            Ec.Index = Index;
            Ec.SubIdx = SubIdx;
            EcatError = TRUE;
            Ec.Etype = EC_ERR_TYPE_SDOINFO_ERROR;
            Ec.AbortCode = AbortCode;
            ec_pusherror(&Ec);
          }

          int ec_SDOread(uint16 slave, uint16 index, uint8 subindex, boolean CA, int *psize, void *p, int timeout)
          {
            ec_SDOt *SDOp, *aSDOp;
            uint16 bytesize, Framedatasize;
            int wkc;
            int32 SDOlen;
            uint8 *bp;
            uint8 *hp;
            ec_mbxbuft MbxIn, MbxOut;
            uint8 cnt, toggle;
            boolean NotLast;

            ec_clearmbx(&MbxIn);
            /* Empty slave out mailbox if something is in. Timout set to 0 */
            wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, 0);
            ec_clearmbx(&MbxOut);
            aSDOp = (ec_SDOt *)&MbxIn;
            SDOp = (ec_SDOt *)&MbxOut;
            SDOp->MbxHeader.length = htoes(0x000a);
            SDOp->MbxHeader.address = htoes(0x0000);
            SDOp->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;
            SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
            SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */
            if (CA)
              SDOp->Command = ECT_SDO_UP_REQ_CA; /* upload request complete access */
            else
              SDOp->Command = ECT_SDO_UP_REQ; /* upload request normal */
            SDOp->Index = htoes(index);
            if (CA && (subindex > 1))
              subindex = 1;
            SDOp->SubIndex = subindex;
            SDOp->ldata[0] = 0;
            /* send CoE request to slave */
            wkc = ec_mbxsend(slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
            if (wkc > 0) /* succeeded to place mailbox in slave ? */
            {
              /* 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 CoE, SDO response and the correct index */
                if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE)
                    && ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && (aSDOp->Index == SDOp->Index))
                {
                  if ((aSDOp->Command & 0x02) > 0)
                  {
                    /* expedited frame response */
                    bytesize = 4 - ((aSDOp->Command >> 2) & 0x03);
                    if (*psize >= bytesize) /* parameter buffer big enough ? */
                    {
                      /* copy parameter in parameter buffer */
                      memcpy(p, &aSDOp->ldata[0], bytesize);
                      /* return the real parameter size */
                      *psize = bytesize;
                    }
                    else
                    {
                      wkc = 0;
                      ec_packeterror(slave, index, subindex, 3); /*  data container too small for type */
                    }
                  }
                  else
                  { /* normal frame response */
                    SDOlen = etohl(aSDOp->ldata[0]);
                    /* Does parameter fit in parameter buffer ? */
                    if (SDOlen <= *psize)
                    {
                      bp = p;
                      hp = p;
                      /* calculate mailbox transfer size */
                      Framedatasize = (etohs(aSDOp->MbxHeader.length) - 10);
                      if (Framedatasize < SDOlen) /* transfer in segments? */
                      {
                        /* copy parameter data in parameter buffer */
                        memcpy(hp, &aSDOp->ldata[1], Framedatasize);
                        /* increment buffer pointer */
                        hp += Framedatasize;
                        *psize = Framedatasize;
                        NotLast = TRUE;
                        toggle= 0x00;
                        while (NotLast) /* segmented transfer */
                        {
                          SDOp = (ec_SDOt *)&MbxOut;
                          SDOp->MbxHeader.length = htoes(0x000a);
                          SDOp->MbxHeader.address = htoes(0x0000);
                          SDOp->MbxHeader.priority = 0x00;
                          cnt = ec_nextmbxcnt(ec_slave[slave].mbx_cnt);
                          ec_slave[slave].mbx_cnt = cnt;
                          SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
                          SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */
                          SDOp->Command = ECT_SDO_SEG_UP_REQ + toggle; /* segment upload request */
                          SDOp->Index = htoes(index);
                          SDOp->SubIndex = subindex;
                          SDOp->ldata[0] = 0;
                          /* send segmented upload request to slave */
                          wkc = ec_mbxsend(slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
                          /* is mailbox transfered to slave ? */
                          if (wkc > 0)
                          {
                            ec_clearmbx(&MbxIn);
                            /* read slave response */
                            wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, timeout);
                            /* has slave responded ? */
                            if (wkc > 0)
                            {
                              /* slave response should be CoE, SDO response */
                              if ((((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) &&
                                      ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) &&
                                      ((aSDOp->Command & 0xe0) == 0x00)))
                              {
                                /* calculate mailbox transfer size */
                                Framedatasize = etohs(aSDOp->MbxHeader.length) - 3;
                                if ((aSDOp->Command & 0x01) > 0)
                                { /* last segment */
                                  NotLast = FALSE;
                                  if (Framedatasize == 7)
                                  /* substract unused bytes from frame */
                                  Framedatasize = Framedatasize - ((aSDOp->Command & 0x0e) >> 1);
                                  /* copy to parameter buffer */
                                  memcpy(hp, &(aSDOp->Index), Framedatasize);
                                }
                                else /* segments follow */
                                {
                                  /* copy to parameter buffer */
                                  memcpy(hp, &(aSDOp->Index), Framedatasize);
                                  /* increment buffer pointer */
                                  hp += Framedatasize;
                                }
                                /* update parametersize */
                                *psize += Framedatasize;
                              }
                              /* unexpected frame returned from slave */
                              else
                              {
                                NotLast = FALSE;
                                if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */
                                ec_SDOerror(slave, index, subindex, etohl(aSDOp->ldata[0]));
                                else
                                ec_packeterror(slave, index, subindex, 1); /* Unexpected frame returned */
                                wkc = 0;
                              }
                            }
                          }
                          toggle = toggle ^ 0x10; /* toggle bit for segment request */
                        }
                      }
                      /* non segmented transfer */
                      else
                      {
                        /* copy to parameter buffer */
                        memcpy(bp, &aSDOp->ldata[1], SDOlen);
                        *psize = SDOlen;
                      }
                    }
                    /* parameter buffer too small */
                    else
                    {
                      wkc = 0;
                      ec_packeterror(slave, index, subindex, 3); /*  data container too small for type */
                    }
                  }
                }
                /* other slave response */
                else
                {
                  if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */
                  ec_SDOerror(slave, index, subindex, etohl(aSDOp->ldata[0]));
                  else
                  ec_packeterror(slave, index, subindex, 1); /* Unexpected frame returned */
                  wkc = 0;
                }
              }
            }
            return wkc;
          }

          int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, boolean CA, int psize, void *p, int Timeout)
          {
            ec_SDOt *SDOp, *aSDOp;
            int wkc, maxdata;
            ec_mbxbuft MbxIn, MbxOut;
            uint8 cnt, toggle;
            uint16 framedatasize;
            boolean NotLast;
            uint8 *hp;

            ec_clearmbx(&MbxIn);
            /* Empty slave out mailbox if something is in. Timout set to 0 */
            wkc = ec_mbxreceive(Slave, (ec_mbxbuft *)&MbxIn, 0);
            ec_clearmbx(&MbxOut);
            aSDOp = (ec_SDOt *)&MbxIn;
            SDOp = (ec_SDOt *)&MbxOut;
            maxdata = ec_slave[Slave].mbx_l - 0x10; /* data section=mailbox size - 6 mbx - 2 CoE - 8 sdo req */
            framedatasize = psize;
            NotLast = FALSE;
            if (framedatasize > maxdata)
            {
              framedatasize = maxdata; /*  segmented transfer needed  */
              NotLast = TRUE;
            }
            SDOp->MbxHeader.length = htoes(0x0a + framedatasize);
            SDOp->MbxHeader.address = htoes(0x0000);
            SDOp->MbxHeader.priority = 0x00;
            /* get new mailbox counter, used for session handle */
            cnt = ec_nextmbxcnt(ec_slave[Slave].mbx_cnt);
            ec_slave[Slave].mbx_cnt = cnt;
            SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
            SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */
            if (CA)
              SDOp->Command = ECT_SDO_DOWN_INIT_CA; /* Complete Access, normal SDO init download transfer */
            else
              SDOp->Command = ECT_SDO_DOWN_INIT; /* normal SDO init download transfer */
            SDOp->Index = htoes(Index);
            SDOp->SubIndex = SubIndex;
            if (CA && (SubIndex > 1))
              SDOp->SubIndex = 1;
            SDOp->ldata[0] = htoel(psize);
            hp = p;
            /* copy parameter data to mailbox */
            memcpy(&SDOp->ldata[1], hp, framedatasize);
            hp += framedatasize;
            psize -= framedatasize;
            /* send mailbox SDO download request to slave */
            wkc = ec_mbxsend(Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
            if (wkc > 0)
            {
              ec_clearmbx(&MbxIn);
              /* read slave response */
              wkc = ec_mbxreceive(Slave, (ec_mbxbuft *)&MbxIn, Timeout);
              if (wkc > 0)
              {
                /* response should be CoE, SDO response, correct index and subindex */
                if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE)
                    && ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && (aSDOp->Index == SDOp->Index)
                    && (aSDOp->SubIndex == SDOp->SubIndex))
                {
                  /* all ok */
                  maxdata += 7;
                  toggle = 0;
                  /* repeat while segments left */
                  while (NotLast)
                  {
                    SDOp = (ec_SDOt *)&MbxOut;
                    framedatasize = psize;
                    NotLast = FALSE;
                    SDOp->Command = 0x01; /* last segment */
                    if (framedatasize > maxdata)
                    {
                      framedatasize = maxdata; /*  more segments needed  */
                      NotLast = TRUE;
                      SDOp->Command = 0x00; /* segments follow */
                    }
                    if (!NotLast && (framedatasize < 7))
                    {
                      SDOp->MbxHeader.length = htoes(0x0a); /* minimum size */
                      SDOp->Command = 0x01 + ((7 - framedatasize) << 1); /* last segment reduced octets */
                    }
                    else
                      SDOp->MbxHeader.length = htoes(framedatasize + 3); /* data + 2 CoE + 1 SDO */
                    SDOp->MbxHeader.address = htoes(0x0000);
                    SDOp->MbxHeader.priority = 0x00;
                    /* get new mailbox counter value */
                    cnt = ec_nextmbxcnt(ec_slave[Slave].mbx_cnt);
                    ec_slave[Slave].mbx_cnt = cnt;
                    SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
                    SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits (SDO request) */
                    SDOp->Command = SDOp->Command + toggle; /* add toggle bit to command byte */
                    /* copy parameter data to mailbox */
                    memcpy(&SDOp->Index, hp, framedatasize);
                    /* update parameter buffer pointer */
                    hp += framedatasize;
                    psize -= framedatasize;
                    /* send SDO download request */
                    wkc = ec_mbxsend(Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
                    if (wkc > 0)
                    {
                      ec_clearmbx(&MbxIn);
                      /* read slave response */
                      wkc = ec_mbxreceive(Slave, (ec_mbxbuft *)&MbxIn, Timeout);
                      if (wkc > 0)
                      {
                        if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE)
                            && ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_SDORES) && ((aSDOp->Command & 0xe0) == 0x20))
                        {
                          /* all OK, nothing to do */
                        }
                        else
                        {
                          if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */
                            ec_SDOerror(Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
                            else
                            ec_packeterror(Slave, Index, SubIndex, 1); /* Unexpected frame returned */
                            wkc = 0;
                            NotLast = FALSE;
                          }
                        }
                      }
                    toggle = toggle ^ 0x10; /* toggle bit for segment request */
                  }
                }
                /* unexpected response from slave */
                else
                {
                  if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */
                    ec_SDOerror(Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
                    else
                    ec_packeterror(Slave, Index, SubIndex, 1); /* Unexpected frame returned */
                    wkc = 0;
                  }
                }
              }

            return wkc;
          }

          int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber, int psize, void *p)
          {
            ec_SDOt *SDOp;
            int wkc, maxdata;
            ec_mbxbuft MbxIn, MbxOut;
            uint8 cnt;
            uint16 framedatasize;

            ec_clearmbx(&MbxIn);
            /* Empty slave out mailbox if something is in. Timout set to 0 */
            wkc = ec_mbxreceive(Slave, (ec_mbxbuft *)&MbxIn, 0);
            ec_clearmbx(&MbxOut);
            SDOp = (ec_SDOt *)&MbxOut;
            maxdata = ec_slave[Slave].mbx_l - 0x08; /* data section=mailbox size - 6 mbx - 2 CoE */
            framedatasize = psize;
            if (framedatasize > maxdata)
              framedatasize = maxdata; /*  limit transfer */
            SDOp->MbxHeader.length = htoes(0x02 + framedatasize);
            SDOp->MbxHeader.address = htoes(0x0000);
            SDOp->MbxHeader.priority = 0x00;
            /* get new mailbox counter, used for session handle */
            cnt = ec_nextmbxcnt(ec_slave[Slave].mbx_cnt);
            ec_slave[Slave].mbx_cnt = cnt;
            SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
            SDOp->CANOpen = htoes((RxPDOnumber & 0x01ff) + (ECT_COES_RXPDO << 12)); /* number 9bits service upper 4 bits */
            /* copy PDO data to mailbox */
            memcpy(&SDOp->Command, p, framedatasize);
            /* send mailbox SDO download request to slave */
            wkc = ec_mbxsend(Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);

            return wkc;
          }

          int ec_TxPDO(uint16 slave, uint16 TxPDOnumber, int *psize, void *p, int timeout)
          {
            ec_SDOt *SDOp, *aSDOp;
            int wkc;
            ec_mbxbuft MbxIn, MbxOut;
            uint8 cnt;
            uint16 framedatasize;

            ec_clearmbx(&MbxIn);
            /* Empty slave out mailbox if something is in. Timout set to 0 */
            wkc = ec_mbxreceive(slave, (ec_mbxbuft *)&MbxIn, 0);
            ec_clearmbx(&MbxOut);
            aSDOp = (ec_SDOt *)&MbxIn;
            SDOp = (ec_SDOt *)&MbxOut;
            SDOp->MbxHeader.length = htoes(0x02);
            SDOp->MbxHeader.address = htoes(0x0000);
            SDOp->MbxHeader.priority = 0x00;
            /* get new mailbox counter, used for session handle */
            cnt = ec_nextmbxcnt(ec_slave[slave].mbx_cnt);
            ec_slave[slave].mbx_cnt = cnt;
            SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
            SDOp->CANOpen = htoes((TxPDOnumber & 0x01ff) + (ECT_COES_TXPDO_RR << 12)); /* number 9bits service upper 4 bits */
            wkc = ec_mbxsend(slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
            if (wkc > 0)
            {
              /* 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 CoE, TxPDO */
                if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE)
                    && ((etohs(aSDOp->CANOpen) >> 12) == ECT_COES_TXPDO))
                {
                  /* TxPDO response */
                  framedatasize = (aSDOp->MbxHeader.length - 2);
                  if (*psize >= framedatasize) /* parameter buffer big enough ? */
                  {
                    /* copy parameter in parameter buffer */
                    memcpy(p, &aSDOp->Command, framedatasize);
                    /* return the real parameter size */
                    *psize = framedatasize;
                  }
                  /* parameter buffer too small */
                  else
                  {
                    wkc = 0;
                    ec_packeterror(slave, 0, 0, 3); /*  data container too small for type */
                  }
                }
                /* other slave response */
                else
                {
                  if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */
                    ec_SDOerror(slave, 0, 0, etohl(aSDOp->ldata[0]));
                    else
                    ec_packeterror(slave, 0, 0, 1); /* Unexpected frame returned */
                    wkc = 0;
                  }
                }
              }

            return wkc;
          }

          int ec_readPDOassign(uint16 Slave, uint16 PDOassign)
          {
            uint16 idxloop, nidx, subidxloop, rdat, idx, subidx;
            uint8 subcnt;
            int wkc, bsize = 0, rdl;
            int32 rdat2;

            rdl = sizeof(rdat);
            rdat = 0;
            /* read PDO assign subindex 0 ( = number of PDO's) */
            wkc = ec_SDOread(Slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
            rdat = etohs(rdat);
            /* positive result from slave ? */
            if ((wkc > 0) && (rdat > 0))
            {
              /* number of available sub indexes */
              nidx = rdat;
              bsize = 0;
              /* read all PDO's */
              for (idxloop = 1; idxloop <= nidx; idxloop++)
              {
                rdl = sizeof(rdat);
                rdat = 0;
                /* read PDO assign */
                wkc = ec_SDOread(Slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
                /* result is index of PDO */
                idx = etohl(rdat);
                if (idx > 0)
                {
                  rdl = sizeof(subcnt);
                  subcnt = 0;
                  /* read number of subindexes of PDO */
                  wkc = ec_SDOread(Slave, idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM);
                  subidx = subcnt;
                  /* for each subindex */
                  for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
                  {
                    rdl = sizeof(rdat2);
                    rdat2 = 0;
                    /* read SDO that is mapped in PDO */
                    wkc = ec_SDOread(Slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM);
                    rdat2 = etohl(rdat2);
                    /* extract bitlength of SDO */
                    if (LO_BYTE(rdat2) < 0xff)
                      bsize += LO_BYTE(rdat2);
                    else
                    {
                      rdl = sizeof(rdat);
                      rdat = htoes(0xff);
                      /* read Object Entry in Object database */
//                                              wkc = ec_readOEsingle(idx, (uint8)SubCount, pODlist, pOElist);
                      bsize += etohs(rdat);
                    }
                  };
                };
              };
            };
            /* return total found bitlength (PDO) */
            return bsize;
          }

          int ec_readPDOassignCA(uint16 Slave, uint16 PDOassign)
          {
            uint16 idxloop, nidx, subidxloop, idx, subidx;
            int wkc, bsize = 0, rdl;

            /* find maximum size of PDOassign buffer */
            rdl = sizeof(ec_PDOassign);
            ec_PDOassign.n = 0;
            /* read rxPDOassign in CA mode, all subindexes are read in one struct */
            wkc = ec_SDOread(Slave, PDOassign, 0x00, TRUE, &rdl, &ec_PDOassign, EC_TIMEOUTRXM);
            /* positive result from slave ? */
            if ((wkc > 0) && (ec_PDOassign.n > 0))
            {
              nidx = ec_PDOassign.n;
              bsize = 0;
              /* for each PDO do */
              for (idxloop = 1; idxloop <= nidx; idxloop++)
              {
                /* get index from PDOassign struct */
                idx = etohs(ec_PDOassign.index[idxloop - 1]);
                if (idx > 0)
                {
                  rdl = sizeof(ec_PDOdesc); ec_PDOdesc.n = 0;
                  /* read SDO's that are mapped in PDO, CA mode */
                  wkc = ec_SDOread(Slave,idx, 0x00, TRUE, &rdl, &ec_PDOdesc, EC_TIMEOUTRXM);
                  subidx = ec_PDOdesc.n;
                  /* extract all bitlengths of SDO's */
                  for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
                  {
                    bsize += LO_BYTE(etohl(ec_PDOdesc.PDO[subidxloop -1]));
                  };
                };
              };
            };
            /* return total found bitlength (PDO) */
            return bsize;
          }

          int ec_readPDOmap(uint16 Slave, int *Osize, int *Isize)
          {
            int wkc, rdl;
            int retVal = 0;
            uint8 nSM, iSM, tSM;
            int Tsize;
            uint8 SMt_bug_add;

            *Isize = 0;
            *Osize = 0;
            SMt_bug_add = 0;
            rdl = sizeof(nSM);
            nSM = 0;
            /* read SyncManager Communication Type object count */
            wkc = ec_SDOread(Slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM);
            /* positive result from slave ? */
            if ((wkc > 0) && (nSM > 2))
            {
              /* make nSM equal to number of defined SM */
              nSM--;
              /* limit to maximum number of SM defined, if true the slave can't be configured */
              if (nSM > EC_MAXSM)
                nSM = EC_MAXSM;
              /* iterate for every SM type defined */
              for (iSM = 2; iSM <= nSM; iSM++)
              {
                rdl = sizeof(tSM);
                tSM = 0;
                /* read SyncManager Communication Type */
                wkc = ec_SDOread(Slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM);
                if (wkc > 0)
                {
// start slave bug prevention code, remove if possible                          
                  if ((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave!
                    SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4
                  if (tSM)
                    tSM += SMt_bug_add; // only add if SMt > 0
// end slave bug prevention code                                

                  ec_slave[Slave].SMtype[iSM] = tSM;
                  /* check if SM is unused -> clear enable flag */
                  if (tSM == 0)
                    ec_slave[Slave].SM[iSM].SMflags = htoel( etohl(ec_slave[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK);
                  if ((tSM == 3) || (tSM == 4))
                  {
                    /* read the assign PDO */
                    Tsize = ec_readPDOassign(Slave, ECT_SDO_PDOASSIGN + iSM);
                    /* if a mapping is found */
                    if (Tsize)
                    {
                      ec_slave[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8);
                      if (tSM == 3)
                        /* we are doing outputs */
                        *Osize += Tsize;
                      else
                        /* we are doing inputs */
                        *Isize += Tsize;
                    }
                  }
                }
              }
            }

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

          int ec_readPDOmapCA(uint16 Slave, int *Osize, int *Isize)
          {
            int wkc, rdl;
            int retVal = 0;
            uint8 nSM, iSM, tSM;
            int Tsize;
            uint8 SMt_bug_add;

            *Isize = 0;
            *Osize = 0;
            SMt_bug_add = 0;
            rdl = sizeof(ec_SMcommtype);
            ec_SMcommtype.n = 0;
            /* read SyncManager Communication Type object count Complete Access*/
            wkc = ec_SDOread(Slave, ECT_SDO_SMCOMMTYPE, 0x00, TRUE, &rdl, &ec_SMcommtype, EC_TIMEOUTRXM);
            /* positive result from slave ? */
            if ((wkc > 0) && (ec_SMcommtype.n > 2))
            {
              /* make nSM equal to number of defined SM */
              nSM = ec_SMcommtype.n - 1;
              /* limit to maximum number of SM defined, if true the slave can't be configured */
              if (nSM > EC_MAXSM)
                nSM = EC_MAXSM;
              /* iterate for every SM type defined */
              for (iSM = 2; iSM <= nSM; iSM++)
              {
                tSM = ec_SMcommtype.SMtype[iSM];

// start slave bug prevention code, remove if possible                          
                if ((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave!
                  SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4
                if (tSM)
                  tSM += SMt_bug_add; // only add if SMt > 0
// end slave bug prevention code

                ec_slave[Slave].SMtype[iSM] = tSM;
                /* check if SM is unused -> clear enable flag */
                if (tSM == 0)
                  ec_slave[Slave].SM[iSM].SMflags = htoel( etohl(ec_slave[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK);
                if ((tSM == 3) || (tSM == 4))
                {
                  /* read the assign PDO */
                  Tsize = ec_readPDOassignCA(Slave, ECT_SDO_PDOASSIGN + iSM);
                  /* if a mapping is found */
                  if (Tsize)
                  {
                    ec_slave[Slave].SM[iSM].SMlength = htoes((Tsize + 7) / 8);
                    if (tSM == 3)
                      /* we are doing outputs */
                      *Osize += Tsize;
                    else
                      /* we are doing inputs */
                      *Isize += Tsize;
                  }
                }
              }
            }

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

          int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist)
          {
            ec_SDOservicet *SDOp, *aSDOp;
            ec_mbxbuft MbxIn, MbxOut;
            int wkc;
            uint16 x, n, i, sp, offset;
            boolean stop;
            uint8 cnt;
            boolean First;

            pODlist->Slave = Slave;
            pODlist->Entries = 0;
            ec_clearmbx(&MbxIn);
            /* clear pending out mailbox in slave if available. Timeout is set to 0 */
            wkc = ec_mbxreceive(Slave, &MbxIn, 0);
            ec_clearmbx(&MbxOut);
            aSDOp = (ec_SDOservicet*)&MbxIn;
            SDOp = (ec_SDOservicet*)&MbxOut;
            SDOp->MbxHeader.length = htoes(0x0008);
            SDOp->MbxHeader.address = htoes(0x0000);
            SDOp->MbxHeader.priority = 0x00;
            /* Get new mailbox counter value */
            cnt = ec_nextmbxcnt(ec_slave[Slave].mbx_cnt);
            ec_slave[Slave].mbx_cnt = cnt;
            SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
            SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */
            SDOp->Opcode = ECT_GET_ODLIST_REQ; /* get object description list request */
            SDOp->Reserved = 0;
            SDOp->Fragments = 0; /* fragments left */
            SDOp->wdata[0] = htoes(0x01); /* all objects */
            /* send get object description list request to slave */
            wkc = ec_mbxsend(Slave, &MbxOut, EC_TIMEOUTTXM);
            /* mailbox placed in slave ? */
            if (wkc > 0)
            {
              x = 0;
              sp = 0;
              First = TRUE;
              offset = 1; /* offset to skip info header in first frame, otherwise set to 0 */
              do
              {
                stop = TRUE; /* assume this is last iteration */
                ec_clearmbx(&MbxIn);
                /* read slave response */
                wkc = ec_mbxreceive(Slave, &MbxIn, EC_TIMEOUTRXM);
                /* got response ? */
                if (wkc > 0)
                {
                  /* response should be CoE and "get object description list response" */
                  if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE)
                      && ((aSDOp->Opcode & 0x7f) == ECT_GET_ODLIST_RES))
                  {
                    if (First)
                      /* extract number of indexes from mailbox data size */
                      n = (etohs(aSDOp->MbxHeader.length) - (6 + 2)) / 2;
                    else
                      /* extract number of indexes from mailbox data size */
                      n = (etohs(aSDOp->MbxHeader.length) - 6) / 2;
                    /* check if indexes fit in buffer structure */
                    if ((sp + n) > EC_MAXODLIST)
                    {
                      n = EC_MAXODLIST + 1 - sp;
                      ec_SDOinfoerror(Slave, 0, 0, 0xf000000); /* Too many entries for master buffer */
                      stop = TRUE;
                    }
                    /* trim to maximum number of ODlist entries defined */
                    if ((pODlist->Entries + n) > EC_MAXODLIST)
                      n = EC_MAXODLIST - pODlist->Entries;
                    pODlist->Entries += n;
                    /* extract indexes one by one */
                    for (i = 0; i < n; i++)
                      pODlist->Index[sp + i] = etohs(aSDOp->wdata[i + offset]);
                    sp += n;
                    /* check if more fragments will follow */
                    if (aSDOp->Fragments > 0)
                      stop = FALSE;
                    First = FALSE;
                    offset = 0;
                  }
                  /* got unexpected response from slave */
                  else
                  {
                    if ((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR) /* SDO info error received */
                    {
                      ec_SDOinfoerror(Slave, 0, 0, etohl(aSDOp->ldata[0]));
                      stop = TRUE;
                    }
                    else
                    ec_packeterror(Slave, 0, 0, 1); /* Unexpected frame returned */
                    wkc = 0;
                    x += 20;
                  }
                }
                x++;
              } while ((x <= 128) && !stop);
            }
            return wkc;
          }

          int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist)
          {
            ec_SDOservicet *SDOp, *aSDOp;
            int wkc;
            uint16 n, Slave;
            ec_mbxbuft MbxIn, MbxOut;
            uint8 cnt;

            Slave = pODlist->Slave;
            pODlist->DataType[Item] = 0;
            pODlist->ObjectCode[Item] = 0;
            pODlist->MaxSub[Item] = 0;
            pODlist->Name[Item][0] = 0;
            ec_clearmbx(&MbxIn);
            /* clear pending out mailbox in slave if available. Timeout is set to 0 */
            wkc = ec_mbxreceive(Slave, &MbxIn, 0);
            ec_clearmbx(&MbxOut);
            aSDOp = (ec_SDOservicet*)&MbxIn;
            SDOp = (ec_SDOservicet*)&MbxOut;
            SDOp->MbxHeader.length = htoes(0x0008);
            SDOp->MbxHeader.address = htoes(0x0000);
            SDOp->MbxHeader.priority = 0x00;
            /* Get new mailbox counter value */
            cnt = ec_nextmbxcnt(ec_slave[Slave].mbx_cnt);
            ec_slave[Slave].mbx_cnt = cnt;
            SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
            SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */
            SDOp->Opcode = ECT_GET_OD_REQ; /* get object description request */
            SDOp->Reserved = 0;
            SDOp->Fragments = 0; /* fragments left */
            SDOp->wdata[0] = htoes(pODlist->Index[Item]); /* Data of Index */
            /* send get object description request to slave */
            wkc = ec_mbxsend(Slave, &MbxOut, EC_TIMEOUTTXM);
            /* mailbox placed in slave ? */
            if (wkc > 0)
            {
              ec_clearmbx(&MbxIn);
              /* read slave response */
              wkc = ec_mbxreceive(Slave, &MbxIn, EC_TIMEOUTRXM);
              /* got response ? */
              if (wkc > 0)
              {
                if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && ((aSDOp->Opcode & 0x7f) == ECT_GET_OD_RES))
                {
                  n = (etohs(aSDOp->MbxHeader.length) - 12); /* length of string(name of object) */
                  if (n > EC_MAXNAME)
                    n = EC_MAXNAME; /* max chars */
                  pODlist->DataType[Item] = etohs(aSDOp->wdata[1]);
                  pODlist->ObjectCode[Item] = aSDOp->bdata[5];
                  pODlist->MaxSub[Item] = aSDOp->bdata[4];

                  strncpy(pODlist->Name[Item], (char *)&aSDOp->bdata[6], n);
                  pODlist->Name[Item][n] = 0x00; /* String terminator */
                }
                /* got unexpected response from slave */
                else
                {
                  if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */
                    ec_SDOinfoerror(Slave, pODlist->Index[Item], 0, etohl(aSDOp->ldata[0]));
                    else
                    ec_packeterror(Slave,pODlist->Index[Item], 0, 1); /* Unexpected frame returned */
                    wkc = 0;
                  }
                }
              }
            return wkc;
          }

          static int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist)
          {
            ec_SDOservicet *SDOp, *aSDOp;
            uint16 wkc, Index, Slave;
            int16 n;
            ec_mbxbuft MbxIn, MbxOut;
            uint8 cnt;

            wkc = 0;
            Slave = pODlist->Slave;
            Index = pODlist->Index[Item];
            ec_clearmbx(&MbxIn);
            /* clear pending out mailbox in slave if available. Timeout is set to 0 */
            wkc = ec_mbxreceive(Slave, &MbxIn, 0);
            ec_clearmbx(&MbxOut);
            aSDOp = (ec_SDOservicet*)&MbxIn;
            SDOp = (ec_SDOservicet*)&MbxOut;
            SDOp->MbxHeader.length = htoes(0x000a);
            SDOp->MbxHeader.address = htoes(0x0000);
            SDOp->MbxHeader.priority = 0x00;
            /* Get new mailbox counter value */
            cnt = ec_nextmbxcnt(ec_slave[Slave].mbx_cnt);
            ec_slave[Slave].mbx_cnt = cnt;
            SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + (cnt << 4); /* CoE */
            SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOINFO << 12)); /* number 9bits service upper 4 bits */
            SDOp->Opcode = ECT_GET_OE_REQ; /* get object entry description request */
            SDOp->Reserved = 0;
            SDOp->Fragments = 0; /* fragments left */
            SDOp->wdata[0] = htoes(Index); /* Index */
            SDOp->bdata[2] = SubI; /* SubIndex */
            SDOp->bdata[3] = 1 + 2 + 4; /* get access rights, object category, PDO */
            /* send get object entry description request to slave */
            wkc = ec_mbxsend(Slave, &MbxOut, EC_TIMEOUTTXM);
            /* mailbox placed in slave ? */
            if (wkc > 0)
            {
              ec_clearmbx(&MbxIn);
              /* read slave response */
              wkc = ec_mbxreceive(Slave, &MbxIn, EC_TIMEOUTRXM);
              /* got response ? */
              if (wkc > 0)
              {
                if (((aSDOp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_COE) && ((aSDOp->Opcode & 0x7f) == ECT_GET_OE_RES))
                {
                  pOElist->Entries++;
                  n = (etohs(aSDOp->MbxHeader.length) - 16); /* length of string(name of object) */
                  if (n > EC_MAXNAME)
                    n = EC_MAXNAME; /* max string length */
                  if (n < 0)
                    n = 0;
                  pOElist->ValueInfo[SubI] = aSDOp->bdata[3];
                  pOElist->DataType[SubI] = etohs(aSDOp->wdata[2]);
                  pOElist->BitLength[SubI] = etohs(aSDOp->wdata[3]);
                  pOElist->ObjAccess[SubI] = etohs(aSDOp->wdata[4]);

                  strncpy(pOElist->Name[SubI], (char *)&aSDOp->wdata[5], n);
                  pOElist->Name[SubI][n] = 0x00; /* string terminator */
                }
                /* got unexpected response from slave */
                else
                {
                  if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */
                    ec_SDOinfoerror(Slave, Index, SubI, etohl(aSDOp->ldata[0]));
                    else
                    ec_packeterror(Slave, Index, SubI, 1); /* Unexpected frame returned */
                    wkc = 0;
                  }
                }
              }
            return wkc;
          }

          int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist)
          {
            uint16 SubCount;
            int wkc;
            uint8 SubI;

            wkc = 0;
            pOElist->Entries = 0;
            SubI = pODlist->MaxSub[Item];
            /* for each entry found in ODlist */
            for (SubCount = 0; SubCount <= (SubI); SubCount++)
              /* read subindex of entry */
              wkc = ec_readOEsingle(Item, (uint8)SubCount, pODlist, pOElist);
            return wkc;
          }