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

/** \file
 * \brief
 * CAN over EtherCAT (CoE) module.
 *
 * SDO read / write and SDO service functions
 */

#include <stdio.h>
#include <string.h>
#include "osal.h"
#include "oshw.h"
#include "ethercattype.h"
#include "ethercatbase.h"
#include "ethercatmain.h"
#include "ethercatcoe.h"

/** SDO structure, not to be confused with EcSDOserviceT */
PACKED_BEGIN
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;
PACKED_END

/** SDO service structure */
PACKED_BEGIN
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;
PACKED_END

/** Report SDO error.
 *
 * @param[in]  context    = context struct
 * @param[in]  Slave      = Slave number
 * @param[in]  Index      = Index that generated error
 * @param[in]  SubIdx     = Subindex that generated error
 * @param[in]  AbortCode  = Abortcode, see EtherCAT documentation for list
 */
void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
{
   ec_errort Ec;

   memset(&Ec, 0, sizeof(Ec));
   Ec.Time = osal_current_time();
   Ec.Slave = Slave;
   Ec.Index = Index;
   Ec.SubIdx = SubIdx;
   *(context->ecaterror) = TRUE;
   Ec.Etype = EC_ERR_TYPE_SDO_ERROR;
   Ec.AbortCode = AbortCode;
   ecx_pusherror(context, &Ec);
}

/** Report SDO info error
 *
 * @param[in]  context    = context struct
 * @param[in]  Slave      = Slave number
 * @param[in]  Index      = Index that generated error
 * @param[in]  SubIdx     = Subindex that generated error
 * @param[in]  AbortCode  = Abortcode, see EtherCAT documentation for list
 */
static void ecx_SDOinfoerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
{
   ec_errort Ec;

   memset(&Ec, 0, sizeof(Ec));
   Ec.Slave = Slave;
   Ec.Index = Index;
   Ec.SubIdx = SubIdx;
   *(context->ecaterror) = TRUE;
   Ec.Etype = EC_ERR_TYPE_SDOINFO_ERROR;
   Ec.AbortCode = AbortCode;
   ecx_pusherror(context, &Ec);
}

/** CoE SDO read, blocking. Single subindex or Complete Access.
 *
 * Only a "normal" upload request is issued. If the requested parameter is <= 4bytes
 * then a "expedited" response is returned, otherwise a "normal" response. If a "normal"
 * response is larger than the mailbox size then the response is segmented. The function
 * will combine all segments and copy them to the parameter buffer.
 *
 * @param[in]  context    = context struct
 * @param[in]  slave      = Slave number
 * @param[in]  index      = Index to read
 * @param[in]  subindex   = Subindex to read, must be 0 or 1 if CA is used.
 * @param[in]  CA         = FALSE = single subindex. TRUE = Complete Access, all subindexes read.
 * @param[in,out] psize   = Size in bytes of parameter buffer, returns bytes read from SDO.
 * @param[out] p          = Pointer to parameter buffer
 * @param[in]  timeout    = Timeout in us, standard is EC_TIMEOUTRXM
 * @return Workcounter from last slave response
 */
int ecx_SDOread(ecx_contextt *context, 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. Timeout set to 0 */
   wkc = ecx_mbxreceive(context, 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(context->slavelist[slave].mbx_cnt);
   context->slavelist[slave].mbx_cnt = cnt;
   SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* 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 = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
   if (wkc > 0) /* succeeded to place mailbox in slave ? */
   {
      /* clean mailboxbuffer */
      ec_clearmbx(&MbxIn);
      /* read slave response */
      wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
      if (wkc > 0) /* succeeded to read slave response ? */
      {
         /* slave response should be 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;
                  ecx_packeterror(context, 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(context->slavelist[slave].mbx_cnt);
                        context->slavelist[slave].mbx_cnt = cnt;
                        SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* 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 = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
                        /* is mailbox transferred to slave ? */
                        if (wkc > 0)
                        {
                           ec_clearmbx(&MbxIn);
                           /* read slave response */
                           wkc = ecx_mbxreceive(context, 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)
                                       /* subtract 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 parameter size */
                                 *psize += Framedatasize;
                              }
                              /* unexpected frame returned from slave */
                              else
                              {
                                 NotLast = FALSE;
                                 if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */
                                    ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0]));
                                 else
                                    ecx_packeterror(context, 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;
                  ecx_packeterror(context, slave, index, subindex, 3); /*  data container too small for type */
               }
            }
         }
         /* other slave response */
         else
         {
            if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */
            {
               ecx_SDOerror(context, slave, index, subindex, etohl(aSDOp->ldata[0]));
            }
            else
            {
               ecx_packeterror(context, slave, index, subindex, 1); /* Unexpected frame returned */
            }
            wkc = 0;
         }
      }
   }
   return wkc;
}

/** CoE SDO write, blocking. Single subindex or Complete Access.
 *
 * A "normal" download request is issued, unless we have
 * small data, then a "expedited" transfer is used. If the parameter is larger than
 * the mailbox size then the download is segmented. The function will split the
 * parameter data in segments and send them to the slave one by one.
 *
 * @param[in]  context    = context struct
 * @param[in]  Slave      = Slave number
 * @param[in]  Index      = Index to write
 * @param[in]  SubIndex   = Subindex to write, must be 0 or 1 if CA is used.
 * @param[in]  CA         = FALSE = single subindex. TRUE = Complete Access, all subindexes written.
 * @param[in]  psize      = Size in bytes of parameter buffer.
 * @param[out] p          = Pointer to parameter buffer
 * @param[in]  Timeout    = Timeout in us, standard is EC_TIMEOUTRXM
 * @return Workcounter from last slave response
 */
int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex,
                 boolean CA, int psize, const void *p, int Timeout)
{
   ec_SDOt *SDOp, *aSDOp;
   int wkc, maxdata, framedatasize;
   ec_mbxbuft MbxIn, MbxOut;
   uint8 cnt, toggle;
   boolean  NotLast;
   const uint8 *hp;

   ec_clearmbx(&MbxIn);
   /* Empty slave out mailbox if something is in. Timeout set to 0 */
   wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0);
   ec_clearmbx(&MbxOut);
   aSDOp = (ec_SDOt *)&MbxIn;
   SDOp = (ec_SDOt *)&MbxOut;
   maxdata = context->slavelist[Slave].mbx_l - 0x10; /* data section=mailbox size - 6 mbx - 2 CoE - 8 sdo req */
   /* if small data use expedited transfer */
   if ((psize <= 4) && !CA)
   {
      SDOp->MbxHeader.length = htoes(0x000a);
      SDOp->MbxHeader.address = htoes(0x0000);
      SDOp->MbxHeader.priority = 0x00;
      /* get new mailbox counter, used for session handle */
      cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
      context->slavelist[Slave].mbx_cnt = cnt;
      SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */
      SDOp->CANOpen = htoes(0x000 + (ECT_COES_SDOREQ << 12)); /* number 9bits service upper 4 bits */
      SDOp->Command = ECT_SDO_DOWN_EXP | (((4 - psize) << 2) & 0x0c); /* expedited SDO download transfer */
      SDOp->Index = htoes(Index);
      SDOp->SubIndex = SubIndex;
      hp = p;
      /* copy parameter data to mailbox */
      memcpy(&SDOp->ldata[0], hp, psize);
      /* send mailbox SDO download request to slave */
      wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
      if (wkc > 0)
      {
         ec_clearmbx(&MbxIn);
         /* read slave response */
         wkc = ecx_mbxreceive(context, 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 */
            }
            /* unexpected response from slave */
            else
            {
               if (aSDOp->Command == ECT_SDO_ABORT) /* SDO abort frame received */
               {
                  ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
               }
               else
               {
                  ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */
               }
               wkc = 0;
            }
         }
      }
   }
   else
   {
      framedatasize = psize;
      NotLast = FALSE;
      if (framedatasize > maxdata)
      {
         framedatasize = maxdata;  /*  segmented transfer needed  */
         NotLast = TRUE;
      }
      SDOp->MbxHeader.length = htoes((uint16)(0x0a + framedatasize));
      SDOp->MbxHeader.address = htoes(0x0000);
      SDOp->MbxHeader.priority = 0x00;
      /* get new mailbox counter, used for session handle */
      cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
      context->slavelist[Slave].mbx_cnt = cnt;
      SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* 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 = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
      if (wkc > 0)
      {
         ec_clearmbx(&MbxIn);
         /* read slave response */
         wkc = ecx_mbxreceive(context, 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 = (uint8)(0x01 + ((7 - framedatasize) << 1)); /* last segment reduced octets */
                  }
                  else
                  {
                     SDOp->MbxHeader.length = htoes((uint16)(framedatasize + 3)); /* data + 2 CoE + 1 SDO */
                  }
                  SDOp->MbxHeader.address = htoes(0x0000);
                  SDOp->MbxHeader.priority = 0x00;
                  /* get new mailbox counter value */
                  cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
                  context->slavelist[Slave].mbx_cnt = cnt;
                  SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* 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 = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
                  if (wkc > 0)
                  {
                     ec_clearmbx(&MbxIn);
                     /* read slave response */
                     wkc = ecx_mbxreceive(context, 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 */
                           {
                              ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
                           }
                           else
                           {
                              ecx_packeterror(context, 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 */
               {
                  ecx_SDOerror(context, Slave, Index, SubIndex, etohl(aSDOp->ldata[0]));
               }
               else
               {
                  ecx_packeterror(context, Slave, Index, SubIndex, 1); /* Unexpected frame returned */
               }
               wkc = 0;
            }
         }
      }
   }

   return wkc;
}

/** CoE RxPDO write, blocking.
 *
 * A RxPDO download request is issued.
 *
 * @param[in]  context       = context struct
 * @param[in]  Slave         = Slave number
 * @param[in]  RxPDOnumber   = Related RxPDO number
 * @param[in]  psize         = Size in bytes of PDO buffer.
 * @param[out] p             = Pointer to PDO buffer
 * @return Workcounter from last slave response
 */
int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber, int psize, const void *p)
{
   ec_SDOt *SDOp;
   int wkc, maxdata, framedatasize;
   ec_mbxbuft MbxIn, MbxOut;
   uint8 cnt;

   ec_clearmbx(&MbxIn);
   /* Empty slave out mailbox if something is in. Timeout set to 0 */
   wkc = ecx_mbxreceive(context, Slave, (ec_mbxbuft *)&MbxIn, 0);
   ec_clearmbx(&MbxOut);
   SDOp = (ec_SDOt *)&MbxOut;
   maxdata = context->slavelist[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((uint16)(0x02 + framedatasize));
   SDOp->MbxHeader.address = htoes(0x0000);
   SDOp->MbxHeader.priority = 0x00;
   /* get new mailbox counter, used for session handle */
   cnt = ec_nextmbxcnt(context->slavelist[Slave].mbx_cnt);
   context->slavelist[Slave].mbx_cnt = cnt;
   SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* 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 RxPDO request to slave */
   wkc = ecx_mbxsend(context, Slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);

   return wkc;
}

/** CoE TxPDO read remote request, blocking.
 *
 * A TxPDO download request is issued.
 *
 * @param[in]  context       = context struct
 * @param[in]  slave         = Slave number
 * @param[in]  TxPDOnumber   = Related TxPDO number
 * @param[in,out] psize      = Size in bytes of PDO buffer, returns bytes read from PDO.
 * @param[out] p             = Pointer to PDO buffer
 * @param[in]  timeout       = Timeout in us, standard is EC_TIMEOUTRXM
 * @return Workcounter from last slave response
 */
int ecx_TxPDO(ecx_contextt *context, 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. Timeout set to 0 */
   wkc = ecx_mbxreceive(context, 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(context->slavelist[slave].mbx_cnt);
   context->slavelist[slave].mbx_cnt = cnt;
   SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* CoE */
   SDOp->CANOpen = htoes((TxPDOnumber & 0x01ff) + (ECT_COES_TXPDO_RR << 12)); /* number 9bits service upper 4 bits */
   wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
   if (wkc > 0)
   {
      /* clean mailboxbuffer */
      ec_clearmbx(&MbxIn);
      /* read slave response */
      wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
      if (wkc > 0) /* succeeded to read slave response ? */
      {
         /* slave response should be 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;
               ecx_packeterror(context, slave, 0, 0, 3); /*  data container too small for type */
            }
         }
         /* other slave response */
         else
         {
            if ((aSDOp->Command) == ECT_SDO_ABORT) /* SDO abort frame received */
            {
               ecx_SDOerror(context, slave, 0, 0, etohl(aSDOp->ldata[0]));
            }
            else
            {
               ecx_packeterror(context, slave, 0, 0, 1); /* Unexpected frame returned */
            }
            wkc = 0;
         }
      }
   }

   return wkc;
}

/** Read PDO assign structure
 * @param[in]  context       = context struct
 * @param[in]  Slave         = Slave number
 * @param[in]  PDOassign     = PDO assign object
 * @return total bitlength of PDO assign
 */
uint32 ecx_readPDOassign(ecx_contextt *context, uint16 Slave, uint16 PDOassign)
{
   uint16 idxloop, nidx, subidxloop, rdat, idx, subidx;
   uint8 subcnt;
   int wkc, rdl;
   int32 rdat2;
   uint32 bsize = 0;

   rdl = sizeof(rdat); rdat = 0;
   /* read PDO assign subindex 0 ( = number of PDO's) */
   wkc = ecx_SDOread(context, 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 = ecx_SDOread(context, Slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
         /* result is index of PDO */
         idx = etohs(rdat);
         if (idx > 0)
         {
            rdl = sizeof(subcnt); subcnt = 0;
            /* read number of subindexes of PDO */
            wkc = ecx_SDOread(context, 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 = ecx_SDOread(context, 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;
}

/** Read PDO assign structure in Complete Access mode
 * @param[in]  context       = context struct
 * @param[in]  Slave         = Slave number
 * @param[in]  Thread_n      = Calling thread index
 * @param[in]  PDOassign     = PDO assign object
 * @return total bitlength of PDO assign
 */
uint32 ecx_readPDOassignCA(ecx_contextt *context, uint16 Slave, int Thread_n,
      uint16 PDOassign)
{
   uint16 idxloop, nidx, subidxloop, idx, subidx;
   int wkc, rdl;
   uint32 bsize = 0;

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

   /* return total found bitlength (PDO) */
   return bsize;
}

/** CoE read PDO mapping.
 *
 * CANopen has standard indexes defined for PDO mapping. This function
 * tries to read them and collect a full input and output mapping size
 * of designated slave.
 *
 * Principal structure in slave:\n
 * 1C00:00 is number of SM defined\n
 * 1C00:01 SM0 type -> 1C10\n
 * 1C00:02 SM1 type -> 1C11\n
 * 1C00:03 SM2 type -> 1C12\n
 * 1C00:04 SM3 type -> 1C13\n
 * Type 0 = unused, 1 = mailbox in, 2 = mailbox out,
 * 3 = outputs (RxPDO), 4 = inputs (TxPDO).
 *
 * 1C12:00 is number of PDO's defined for SM2\n
 * 1C12:01 PDO assign SDO #1 -> f.e. 1A00\n
 * 1C12:02 PDO assign SDO #2 -> f.e. 1A04\
 *
 * 1A00:00 is number of object defined for this PDO\n
 * 1A00:01 object mapping #1, f.e. 60100710 (SDO 6010 SI 07 bitlength 0x10)
 *
 * @param[in]  context = context struct
 * @param[in]  Slave   = Slave number
 * @param[out] Osize   = Size in bits of output mapping (rxPDO) found
 * @param[out] Isize   = Size in bits of input mapping (txPDO) found
 * @return >0 if mapping successful.
 */
int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, uint32 *Osize, uint32 *Isize)
{
   int wkc, rdl;
   int retVal = 0;
   uint8 nSM, iSM, tSM;
   uint32 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 = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM);
   /* positive result from slave ? */
   if ((wkc > 0) && (nSM > 2))
   {
      /* 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 = ecx_SDOread(context, 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
            }
            if((iSM == 2) && (tSM == 0)) // SM2 has type 0, this is a bug in the slave!
            {
               tSM = 3;
            }
            if((iSM == 3) && (tSM == 0)) // SM3 has type 0, this is a bug in the slave!
            {
               tSM = 4;
            }
// end slave bug prevention code

            context->slavelist[Slave].SMtype[iSM] = tSM;
            /* check if SM is unused -> clear enable flag */
            if (tSM == 0)
            {
               context->slavelist[Slave].SM[iSM].SMflags =
                  htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK);
            }
            if ((tSM == 3) || (tSM == 4))
            {
               /* read the assign PDO */
               Tsize = ecx_readPDOassign(context, Slave, ECT_SDO_PDOASSIGN + iSM );
               /* if a mapping is found */
               if (Tsize)
               {
                  context->slavelist[Slave].SM[iSM].SMlength = htoes((uint16)((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;
}

/** CoE read PDO mapping in Complete Access mode (CA).
 *
 * CANopen has standard indexes defined for PDO mapping. This function
 * tries to read them and collect a full input and output mapping size
 * of designated slave. Slave has to support CA, otherwise use ec_readPDOmap().
 *
 * @param[in]  context  = context struct
 * @param[in]  Slave    = Slave number
 * @param[in]  Thread_n = Calling thread index
 * @param[out] Osize    = Size in bits of output mapping (rxPDO) found
 * @param[out] Isize    = Size in bits of input mapping (txPDO) found
 * @return >0 if mapping successful.
 */
int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int Thread_n, uint32 *Osize, uint32 *Isize)
{
   int wkc, rdl;
   int retVal = 0;
   uint8 nSM, iSM, tSM;
   uint32 Tsize;
   uint8 SMt_bug_add;

   *Isize = 0;
   *Osize = 0;
   SMt_bug_add = 0;
   rdl = sizeof(ec_SMcommtypet);
   context->SMcommtype[Thread_n].n = 0;
   /* read SyncManager Communication Type object count Complete Access*/
   wkc = ecx_SDOread(context, Slave, ECT_SDO_SMCOMMTYPE, 0x00, TRUE, &rdl,
         &(context->SMcommtype[Thread_n]), EC_TIMEOUTRXM);
   /* positive result from slave ? */
   if ((wkc > 0) && (context->SMcommtype[Thread_n].n > 2))
   {
      nSM = context->SMcommtype[Thread_n].n;
      /* limit to maximum number of SM defined, if true the slave can't be configured */
      if (nSM > EC_MAXSM)
      {
         nSM = EC_MAXSM;
         ecx_packeterror(context, Slave, 0, 0, 10); /* #SM larger than EC_MAXSM */
      }
      /* iterate for every SM type defined */
      for (iSM = 2 ; iSM < nSM ; iSM++)
      {
         tSM = context->SMcommtype[Thread_n].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

         context->slavelist[Slave].SMtype[iSM] = tSM;
         /* check if SM is unused -> clear enable flag */
         if (tSM == 0)
         {
            context->slavelist[Slave].SM[iSM].SMflags =
               htoel( etohl(context->slavelist[Slave].SM[iSM].SMflags) & EC_SMENABLEMASK);
         }
         if ((tSM == 3) || (tSM == 4))
         {
            /* read the assign PDO */
            Tsize = ecx_readPDOassignCA(context, Slave, Thread_n,
                  ECT_SDO_PDOASSIGN + iSM );
            /* if a mapping is found */
            if (Tsize)
            {
               context->slavelist[Slave].SM[iSM].SMlength = htoes((uint16)((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;
}

/** CoE read Object Description List.
 *
 * @param[in]  context  = context struct
 * @param[in]  Slave    = Slave number.
 * @param[out] pODlist  = resulting Object Description list.
 * @return Workcounter of slave response.
 */
int ecx_readODlist(ecx_contextt *context, 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 = ecx_mbxreceive(context, 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(context->slavelist[Slave].mbx_cnt);
   context->slavelist[Slave].mbx_cnt = cnt;
   SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* 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 = ecx_mbxsend(context, 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 = ecx_mbxreceive(context, 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;
                  ecx_SDOinfoerror(context, 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 */
               {
                  ecx_SDOinfoerror(context, Slave, 0, 0, etohl(aSDOp->ldata[0]));
                  stop = TRUE;
               }
               else
               {
                  ecx_packeterror(context, Slave, 0, 0, 1); /* Unexpected frame returned */
               }
               wkc = 0;
               x += 20;
            }
         }
         x++;
      }
      while ((x <= 128) && !stop);
   }
   return wkc;
}

/** CoE read Object Description. Adds textual description to object indexes.
 *
 * @param[in]  context       = context struct
 * @param[in] Item           = Item number in ODlist.
 * @param[in,out] pODlist    = referencing Object Description list.
 * @return Workcounter of slave response.
 */
int ecx_readODdescription(ecx_contextt *context, 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 = ecx_mbxreceive(context, 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(context->slavelist[Slave].mbx_cnt);
   context->slavelist[Slave].mbx_cnt = cnt;
   SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* 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 = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
   /* mailbox placed in slave ? */
   if (wkc > 0)
   {
      ec_clearmbx(&MbxIn);
      /* read slave response */
      wkc = ecx_mbxreceive(context, 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 */
            {
               ecx_SDOinfoerror(context, Slave,pODlist->Index[Item], 0, etohl(aSDOp->ldata[0]));
            }
            else
            {
               ecx_packeterror(context, Slave,pODlist->Index[Item], 0, 1); /* Unexpected frame returned */
            }
            wkc = 0;
         }
      }
   }

   return wkc;
}

/** CoE read SDO service object entry, single subindex.
 * Used in ec_readOE().
 *
 * @param[in]  context       = context struct
 * @param[in] Item           = Item in ODlist.
 * @param[in] SubI           = Subindex of item in ODlist.
 * @param[in] pODlist        = Object description list for reference.
 * @param[out] pOElist       = resulting object entry structure.
 * @return Workcounter of slave response.
 */
int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist)
{
   ec_SDOservicet *SDOp, *aSDOp;
   int wkc;
   uint16 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 = ecx_mbxreceive(context, 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(context->slavelist[Slave].mbx_cnt);
   context->slavelist[Slave].mbx_cnt = cnt;
   SDOp->MbxHeader.mbxtype = ECT_MBXT_COE + MBX_HDR_SET_CNT(cnt); /* 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 = ecx_mbxsend(context, Slave, &MbxOut, EC_TIMEOUTTXM);
   /* mailbox placed in slave ? */
   if (wkc > 0)
   {
      ec_clearmbx(&MbxIn);
      /* read slave response */
      wkc = ecx_mbxreceive(context, 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 */
            {
               ecx_SDOinfoerror(context, Slave, Index, SubI, etohl(aSDOp->ldata[0]));
            }
            else
            {
               ecx_packeterror(context, Slave, Index, SubI, 1); /* Unexpected frame returned */
            }
            wkc = 0;
         }
      }
   }

   return wkc;
}

/** CoE read SDO service object entry.
 *
 * @param[in] context        = context struct
 * @param[in] Item           = Item in ODlist.
 * @param[in] pODlist        = Object description list for reference.
 * @param[out] pOElist       = resulting object entry structure.
 * @return Workcounter of slave response.
 */
int ecx_readOE(ecx_contextt *context, 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 = ecx_readOEsingle(context, Item, (uint8)SubCount, pODlist, pOElist);
   }

   return wkc;
}

#ifdef EC_VER1
/** Report SDO error.
 *
 * @param[in]  Slave      = Slave number
 * @param[in]  Index      = Index that generated error
 * @param[in]  SubIdx     = Subindex that generated error
 * @param[in]  AbortCode  = Abortcode, see EtherCAT documentation for list
 * @see ecx_SDOerror
 */
void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode)
{
   ecx_SDOerror(&ecx_context, Slave, Index, SubIdx, AbortCode);
}

/** CoE SDO read, blocking. Single subindex or Complete Access.
 *
 * Only a "normal" upload request is issued. If the requested parameter is <= 4bytes
 * then a "expedited" response is returned, otherwise a "normal" response. If a "normal"
 * response is larger than the mailbox size then the response is segmented. The function
 * will combine all segments and copy them to the parameter buffer.
 *
 * @param[in]  slave      = Slave number
 * @param[in]  index      = Index to read
 * @param[in]  subindex   = Subindex to read, must be 0 or 1 if CA is used.
 * @param[in]  CA         = FALSE = single subindex. TRUE = Complete Access, all subindexes read.
 * @param[in,out] psize   = Size in bytes of parameter buffer, returns bytes read from SDO.
 * @param[out] p          = Pointer to parameter buffer
 * @param[in]  timeout    = Timeout in us, standard is EC_TIMEOUTRXM
 * @return Workcounter from last slave response
 * @see ecx_SDOread
 */
int ec_SDOread(uint16 slave, uint16 index, uint8 subindex,
               boolean CA, int *psize, void *p, int timeout)
{
   return ecx_SDOread(&ecx_context, slave, index, subindex, CA, psize, p, timeout);
}

/** CoE SDO write, blocking. Single subindex or Complete Access.
 *
 * A "normal" download request is issued, unless we have
 * small data, then a "expedited" transfer is used. If the parameter is larger than
 * the mailbox size then the download is segmented. The function will split the
 * parameter data in segments and send them to the slave one by one.
 *
 * @param[in]  Slave      = Slave number
 * @param[in]  Index      = Index to write
 * @param[in]  SubIndex   = Subindex to write, must be 0 or 1 if CA is used.
 * @param[in]  CA         = FALSE = single subindex. TRUE = Complete Access, all subindexes written.
 * @param[in]  psize      = Size in bytes of parameter buffer.
 * @param[out] p          = Pointer to parameter buffer
 * @param[in]  Timeout    = Timeout in us, standard is EC_TIMEOUTRXM
 * @return Workcounter from last slave response
 * @see ecx_SDOwrite
 */
int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex,
                boolean CA, int psize, const void *p, int Timeout)
{
   return ecx_SDOwrite(&ecx_context, Slave, Index, SubIndex, CA, psize, p, Timeout);
}

/** CoE RxPDO write, blocking.
 *
 * A RxPDO download request is issued.
 *
 * @param[in]  Slave         = Slave number
 * @param[in]  RxPDOnumber   = Related RxPDO number
 * @param[in]  psize         = Size in bytes of PDO buffer.
 * @param[out] p             = Pointer to PDO buffer
 * @return Workcounter from last slave response
 * @see ecx_RxPDO
 */
int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber, int psize, const void *p)
{
   return ecx_RxPDO(&ecx_context, Slave, RxPDOnumber, psize, p);
}

/** CoE TxPDO read remote request, blocking.
 *
 * A TxPDO download request is issued.
 *
 * @param[in]  slave         = Slave number
 * @param[in]  TxPDOnumber   = Related TxPDO number
 * @param[in,out] psize      = Size in bytes of PDO buffer, returns bytes read from PDO.
 * @param[out] p             = Pointer to PDO buffer
 * @param[in]  timeout       = Timeout in us, standard is EC_TIMEOUTRXM
 * @return Workcounter from last slave response
 * @see ecx_TxPDO
 */
int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout)
{
   return ecx_TxPDO(&ecx_context, slave, TxPDOnumber, psize, p, timeout);
}

/** Read PDO assign structure
 * @param[in]  Slave         = Slave number
 * @param[in]  PDOassign     = PDO assign object
 * @return total bitlength of PDO assign
 */
uint32 ec_readPDOassign(uint16 Slave, uint16 PDOassign)
{
   return ecx_readPDOassign(&ecx_context, Slave, PDOassign);
}

/** Read PDO assign structure in Complete Access mode
 * @param[in]  Slave         = Slave number
 * @param[in]  PDOassign     = PDO assign object
 * @param[in]  Thread_n      = Calling thread index
 * @return total bitlength of PDO assign
 * @see ecx_readPDOmap
 */
uint32 ec_readPDOassignCA(uint16 Slave, uint16 PDOassign, int Thread_n)
{
   return ecx_readPDOassignCA(&ecx_context, Slave, Thread_n, PDOassign);
}

/** CoE read PDO mapping.
 *
 * CANopen has standard indexes defined for PDO mapping. This function
 * tries to read them and collect a full input and output mapping size
 * of designated slave.
 *
 * For details, see #ecx_readPDOmap
 *
 * @param[in] Slave    = Slave number
 * @param[out] Osize   = Size in bits of output mapping (rxPDO) found
 * @param[out] Isize   = Size in bits of input mapping (txPDO) found
 * @return >0 if mapping succesful.
 */
int ec_readPDOmap(uint16 Slave, uint32 *Osize, uint32 *Isize)
{
   return ecx_readPDOmap(&ecx_context, Slave, Osize, Isize);
}

/** CoE read PDO mapping in Complete Access mode (CA).
 *
 * CANopen has standard indexes defined for PDO mapping. This function
 * tries to read them and collect a full input and output mapping size
 * of designated slave. Slave has to support CA, otherwise use ec_readPDOmap().
 *
 * @param[in] Slave    = Slave number
 * @param[in] Thread_n = Calling thread index
 * @param[out] Osize   = Size in bits of output mapping (rxPDO) found
 * @param[out] Isize   = Size in bits of input mapping (txPDO) found
 * @return >0 if mapping succesful.
 * @see ecx_readPDOmap ec_readPDOmapCA
 */
int ec_readPDOmapCA(uint16 Slave, int Thread_n, uint32 *Osize, uint32 *Isize)
{
   return ecx_readPDOmapCA(&ecx_context, Slave, Thread_n, Osize, Isize);
}

/** CoE read Object Description List.
 *
 * @param[in] Slave      = Slave number.
 * @param[out] pODlist  = resulting Object Description list.
 * @return Workcounter of slave response.
 * @see ecx_readODlist
 */
int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist)
{
   return ecx_readODlist(&ecx_context, Slave, pODlist);
}

/** CoE read Object Description. Adds textual description to object indexes.
 *
 * @param[in] Item           = Item number in ODlist.
 * @param[in,out] pODlist    = referencing Object Description list.
 * @return Workcounter of slave response.
 * @see ecx_readODdescription
 */
int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist)
{
   return ecx_readODdescription(&ecx_context, Item, pODlist);
}

int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist)
{
   return ecx_readOEsingle(&ecx_context, Item, SubI, pODlist, pOElist);
}

/** CoE read SDO service object entry.
 *
 * @param[in] Item           = Item in ODlist.
 * @param[in] pODlist        = Object description list for reference.
 * @param[out] pOElist       = resulting object entry structure.
 * @return Workcounter of slave response.
 * @see ecx_readOE
 */
int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist)
{
   return ecx_readOE(&ecx_context, Item, pODlist, pOElist);
}
#endif