/*
 * 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 <stddef.h>
#include <string.h>
#include <cc.h>
#include "esc.h"
#include "esc_coe.h"

#define BITS2BYTES(b) ((b + 7) >> 3)

/* Fetch value from object dictionary */
#define OBJ_VALUE_FETCH(v, o) \
   ((o).data ? *(__typeof__ (v) *)(o).data : (__typeof__ (v))(o).value)

/** Search for an object sub-index.
 *
 * @param[in] nidx   = local array index of object we want to find sub-index to
 * @param[in] subindex   = value on sub-index of object we want to locate
 * @return local array index if we succeed, -1 if we didn't find the index.
 */
int16_t SDO_findsubindex (int16_t nidx, uint8_t subindex)
{
   const _objd *objd;
   int16_t n = 0;
   uint8_t maxsub;
   objd = SDOobjects[nidx].objdesc;
   maxsub = SDOobjects[nidx].maxsub;
   while (((objd + n)->subindex < subindex) && (n < maxsub))
   {
      n++;
   }
   if ((objd + n)->subindex != subindex)
   {
      return -1;
   }
   return n;
}

/** Search for an object index matching the wanted value in the Object List.
 *
 * @param[in] index   = value on index of object we want to locate
 * @return local array index if we succeed, -1 if we didn't find the index.
 */
int32_t SDO_findobject (uint16_t index)
{
   int32_t n = 0;
   while (SDOobjects[n].index < index)
   {
      n++;
   }
   if (SDOobjects[n].index != index)
   {
      return -1;
   }
   return n;
}

/**
 * Calculate the size in Bytes of RxPDO or TxPDOs by adding the
 * objects in SyncManager SDO 1C1x.
 *
 * @param[in] index = SM index
 * @param[out] nmappings = number of mapped objects in SM, or -1 if
 *   mapping is invalid
 * @param[out] mappings = list of mapped objects in SM
 * @param[out] max_mappings = max number of mapped objects in SM
 * @return size of RxPDO or TxPDOs in Bytes.
 */
uint16_t sizeOfPDO (uint16_t index, int * nmappings,_SMmap * mappings,
                    int max_mappings)
{
   uint16_t offset = 0, hobj;
   uint8_t si, sic, c;
   int16_t nidx;
   const _objd *objd;
   const _objd *objd1c1x;
   int mapIx = 0;

   if ((index != RX_PDO_OBJIDX) && (index != TX_PDO_OBJIDX))
   {
      return 0;
   }

   nidx = SDO_findobject (index);
   if(nidx < 0)
   {
      return 0;
   }

   objd1c1x = SDOobjects[nidx].objdesc;

   si = OBJ_VALUE_FETCH (si, objd1c1x[0]);
   if (si)
   {
      for (sic = 1; sic <= si; sic++)
      {
         hobj = OBJ_VALUE_FETCH (hobj, objd1c1x[sic]);
         nidx = SDO_findobject (hobj);
         if (nidx >= 0)
         {
            uint8_t maxsub;

            objd = SDOobjects[nidx].objdesc;
            maxsub = OBJ_VALUE_FETCH (maxsub, objd[0]);

            for (c = 1; c <= maxsub; c++)
            {
               uint32_t value = OBJ_VALUE_FETCH (value, objd[c]);
               uint16_t index = value >> 16;
               uint8_t subindex = (value >> 8) & 0xFF;
               uint8_t bitlength = value & 0xFF;
               const _objd * mapping;

               if (mapIx == max_mappings)
               {
                  /* Too many mapped objects */
                  *nmappings = -1;
                  return 0;
               }

               DPRINT ("%04x:%02x @ %d\n", index, subindex, offset);
               nidx = SDO_findobject (index);
               if (nidx >= 0)
               {
                  int16_t nsub;

                  nsub = SDO_findsubindex (nidx, subindex);
                  if (nsub < 0)
                  {
                     mapping = NULL;
                  }

                  mapping = &SDOobjects[nidx].objdesc[nsub];
               }
               else
               {
                  mapping = NULL;
               }

               mappings[mapIx].obj = mapping;
               mappings[mapIx++].offset = offset;

               offset += bitlength;
            }
         }
      }
   }
   *nmappings = mapIx;
   return BITS2BYTES (offset);
}

/** Copy to mailbox.
 *
 * @param[in] source = pointer to source
 * @param[in] dest   = pointer to destination
 * @param[in] size   = Size to copy
 */
void copy2mbx (void *source, void *dest, uint16_t size)
{
   memcpy (dest, source, size);
}

/** Function for sending an SDO Abort reply.
 *
 * @param[in] index      = index of object causing abort reply
 * @param[in] sub-index  = sub-index of object causing abort reply
 * @param[in] abortcode  = abort code to send in reply
 */
void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode)
{
   uint8_t MBXout;
   _COEsdo *coeres;
   MBXout = ESC_claimbuffer ();
   if (MBXout)
   {
      coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE];
      coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
      coeres->mbxheader.mbxtype = MBXCOE;
      coeres->coeheader.numberservice =
         htoes ((0 & 0x01f) | (COE_SDOREQUEST << 12));
      coeres->index = htoes (index);
      coeres->subindex = subindex;
      coeres->command = COE_COMMAND_SDOABORT;
      coeres->size = htoel (abortcode);
      MBXcontrol[MBXout].state = MBXstate_outreq;
   }
}

/** Function for responding on requested SDO Upload, sending the content
 *  requested in a free Mailbox buffer. Depending of size of data expedited,
 *  normal or segmented transfer is used. On error an SDO Abort will be sent.
 */
void SDO_upload (void)
{
   _COEsdo *coesdo, *coeres;
   uint16_t index;
   uint8_t subindex;
   int16_t nidx, nsub;
   uint8_t MBXout;
   uint32_t size;
   uint8_t dss;
   const _objd *objd;
   coesdo = (_COEsdo *) &MBX[0];
   index = etohs (coesdo->index);
   subindex = coesdo->subindex;
   nidx = SDO_findobject (index);
   if (nidx >= 0)
   {
      nsub = SDO_findsubindex (nidx, subindex);
      if (nsub >= 0)
      {
         objd = SDOobjects[nidx].objdesc;
         MBXout = ESC_claimbuffer ();
         if (MBXout)
         {
            coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE];
            coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
            coeres->mbxheader.mbxtype = MBXCOE;
            coeres->coeheader.numberservice =
               htoes ((0 & 0x01f) | (COE_SDORESPONSE << 12));
            size = (objd + nsub)->bitlength;
            /* expedited bits used calculation */
            dss = 0x0c;
            if (size > 8)
            {
               dss = 0x08;
            }
            if (size > 16)
            {
               dss = 0x04;
            }
            if (size > 24)
            {
               dss = 0x00;
            }
            coeres->index = htoes (index);
            coeres->subindex = subindex;
            if (size <= 32)
            {
               /* expedited response i.e. length<=4 bytes */
               coeres->command = COE_COMMAND_UPLOADRESPONSE +
                  COE_SIZE_INDICATOR + COE_EXPEDITED_INDICATOR + dss;
               if ((objd + nsub)->data == NULL)
               {
                  /* use constant value */
                  coeres->size = htoel ((objd + nsub)->value);
               }
               else
               {
                  /* convert bits to bytes */
                  size = (size + 7) >> 3;
                  /* use dynamic data */
                  copy2mbx ((objd + nsub)->data, &(coeres->size), size);
               }
            }
            else
            {
               /* normal response i.e. length>4 bytes */
               coeres->command = COE_COMMAND_UPLOADRESPONSE +
                  COE_SIZE_INDICATOR;
               /* convert bits to bytes */
               size = (size + 7) >> 3;
               coeres->size = htoel (size);
               if ((size + COE_HEADERSIZE) > ESC_MBXDSIZE)
               {
                  /* segmented transfer needed */
                  /* set total size in bytes */
                  ESCvar.frags = size;
                  /* limit to mailbox size */
                  size = ESC_MBXDSIZE - COE_HEADERSIZE;
                  /* number of bytes done */
                  ESCvar.fragsleft = size;
                  /* signal segmented transfer */
                  ESCvar.segmented = MBXSEU;
                  ESCvar.data = (objd + nsub)->data;
               }
               else
               {
                  ESCvar.segmented = 0;
               }
               coeres->mbxheader.length = htoes (COE_HEADERSIZE + size);
               /* use dynamic data */
               copy2mbx ((objd + nsub)->data, (&(coeres->size)) + 1, size);
            }
            MBXcontrol[MBXout].state = MBXstate_outreq;
         }
      }
      else
      {
         SDO_abort (index, subindex, ABORT_NOSUBINDEX);
      }
   }
   else
   {
      SDO_abort (index, subindex, ABORT_NOOBJECT);
   }
   MBXcontrol[0].state = MBXstate_idle;
   ESCvar.xoe = 0;
}

/** Function for handling the following SDO Upload if previous SDOUpload
 * response was flagged it needed to be segmented.
 *
 */
void SDO_uploadsegment (void)
{
   _COEsdo *coesdo, *coeres;
   uint8_t MBXout;
   uint32_t size, offset;
   coesdo = (_COEsdo *) &MBX[0];
   MBXout = ESC_claimbuffer ();
   if (MBXout)
   {
      coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE];
      offset = ESCvar.fragsleft;
      size = ESCvar.frags - ESCvar.fragsleft;
      coeres->mbxheader.mbxtype = MBXCOE;
      coeres->coeheader.numberservice =
         htoes ((0 & 0x01f) | (COE_SDORESPONSE << 12));
      coeres->command = COE_COMMAND_UPLOADSEGMENT + (coesdo->command & COE_TOGGLEBIT);  // copy toggle bit
      if ((size + COE_SEGMENTHEADERSIZE) > ESC_MBXDSIZE)
      {
         /* more segmented transfer needed */
         /* limit to mailbox size */
         size = ESC_MBXDSIZE - COE_SEGMENTHEADERSIZE;
         /* number of bytes done */
         ESCvar.fragsleft += size;
         coeres->mbxheader.length = htoes (COE_SEGMENTHEADERSIZE + size);
      }
      else
      {
         /* last segment */
         ESCvar.segmented = 0;
         ESCvar.frags = 0;
         ESCvar.fragsleft = 0;
         coeres->command += COE_COMMAND_LASTSEGMENTBIT;
         if (size >= 7)
         {
            coeres->mbxheader.length = htoes (COE_SEGMENTHEADERSIZE + size);
         }
         else
         {
            coeres->command += (7 - size) << 1;
            coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
         }
      }
      copy2mbx ((uint8_t *) ESCvar.data + offset, (&(coeres->command)) + 1, size);        //copy to mailbox

      MBXcontrol[MBXout].state = MBXstate_outreq;
   }
   MBXcontrol[0].state = MBXstate_idle;
   ESCvar.xoe = 0;
}


/** Function for handling incoming requested SDO Download, validating the
 * request and sending an response. On error an SDO Abort will be sent.
 */
void SDO_download (void)
{
   _COEsdo *coesdo, *coeres;
   uint16_t index;
   uint8_t subindex;
   int16_t nidx, nsub;
   uint8_t MBXout;
   uint16_t size, actsize;
   const _objd *objd;
   uint32_t *mbxdata;
   uint32_t abort;

   coesdo = (_COEsdo *) &MBX[0];
   index = etohs (coesdo->index);
   subindex = coesdo->subindex;
   nidx = SDO_findobject (index);
   if (nidx >= 0)
   {
      nsub = SDO_findsubindex (nidx, subindex);
      if (nsub >= 0)
      {
         objd = SDOobjects[nidx].objdesc;
         uint8_t access = (objd + nsub)->access & 0x3f;
         uint8_t state = ESCvar.ALstatus & 0x0f;
         if (access == ATYPE_RW ||
             (access == ATYPE_RWpre && state == ESCpreop))
         {
            /* expedited? */
            if (coesdo->command & COE_EXPEDITED_INDICATOR)
            {
               size = 4 - ((coesdo->command & 0x0c) >> 2);
               mbxdata = &(coesdo->size);
            }
            else
            {
               /* normal download */
               size = (etohl (coesdo->size) & 0xffff);
               mbxdata = (&(coesdo->size)) + 1;
            }
            actsize = ((objd + nsub)->bitlength + 7) >> 3;
            if (actsize == size)
            {
               abort = ESC_pre_objecthandler (
                  index,
                  subindex,
                  mbxdata,
                  size,
                  false
               );
               if (abort == 0)
               {
                  copy2mbx (mbxdata, (objd + nsub)->data, size);
                  MBXout = ESC_claimbuffer ();
                  if (MBXout)
                  {
                     coeres = (_COEsdo *) &MBX[MBXout * ESC_MBXSIZE];
                     coeres->mbxheader.length = htoes (COE_DEFAULTLENGTH);
                     coeres->mbxheader.mbxtype = MBXCOE;
                     coeres->coeheader.numberservice =
                        htoes ((0 & 0x01f) | (COE_SDORESPONSE << 12));
                     coeres->index = htoes (index);
                     coeres->subindex = subindex;
                     coeres->command = COE_COMMAND_DOWNLOADRESPONSE;
                     coeres->size = htoel (0);
                     MBXcontrol[MBXout].state = MBXstate_outreq;
                  }
                  /* external object write handler */
                  ESC_objecthandler (index, subindex, false);
               }
               else
               {
                  SDO_abort (index, subindex, abort);
               }
            }
            else
            {
               SDO_abort (index, subindex, ABORT_TYPEMISMATCH);
            }
         }
         else
         {
            if (access == ATYPE_RWpre)
            {
               SDO_abort (index, subindex, ABORT_NOTINTHISSTATE);
            }
            else
            {
               SDO_abort (index, subindex, ABORT_READONLY);
            }
         }
      }
      else
      {
         SDO_abort (index, subindex, ABORT_NOSUBINDEX);
      }
   }
   else
   {
      SDO_abort (index, subindex, ABORT_NOOBJECT);
   }
   MBXcontrol[0].state = MBXstate_idle;
   ESCvar.xoe = 0;
}

/** Function for sending an SDO Info Error reply.
 *
 * @param[in] abortcode  = = abort code to send in reply
 */
void SDO_infoerror (uint32_t abortcode)
{
   uint8_t MBXout;
   _COEobjdesc *coeres;
   MBXout = ESC_claimbuffer ();
   if (MBXout)
   {
      coeres = (_COEobjdesc *) &MBX[MBXout * ESC_MBXSIZE];
      coeres->mbxheader.length = htoes ((uint16_t) 0x0a);
      coeres->mbxheader.mbxtype = MBXCOE;
      coeres->coeheader.numberservice =
         htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
      /* SDO info error request */
      coeres->infoheader.opcode = COE_INFOERROR;
      coeres->infoheader.incomplete = 0;
      coeres->infoheader.reserved = 0x00;
      coeres->infoheader.fragmentsleft = 0;
      coeres->index = (uint16_t)htoel (abortcode);
      coeres->datatype = (uint16_t)(htoel (abortcode) >> 16);
      MBXcontrol[MBXout].state = MBXstate_outreq;
      MBXcontrol[0].state = MBXstate_idle;
      ESCvar.xoe = 0;
   }
}

#define ODLISTSIZE  ((ESC_MBX1_sml - ESC_MBXHSIZE - sizeof(_COEh) - sizeof(_INFOh) - 2) & 0xfffe)

/** Function for handling incoming requested SDO Get OD List, validating the
 * request and sending an response. On error an SDO Info Error will be sent.
 */
void SDO_getodlist (void)
{
   uint16_t frags;
   uint8_t MBXout = 0;
   uint16_t entries = 0;
   uint16_t i, n;
   uint16_t *p;
   _COEobjdesc *coel, *coer;

   while (SDOobjects[entries].index != 0xffff)
   {
      entries++;
   }
   ESCvar.entries = entries;
   frags = ((entries << 1) + ODLISTSIZE - 1);
   frags /= ODLISTSIZE;
   coer = (_COEobjdesc *) &MBX[0];
   /* check for unsupported opcodes */
   if (etohs (coer->index) > 0x01)
   {
      SDO_infoerror (ABORT_UNSUPPORTED);
   }
   else
   {
      MBXout = ESC_claimbuffer ();
   }
   if (MBXout)
   {
      coel = (_COEobjdesc *) &MBX[MBXout * ESC_MBXSIZE];
      coel->mbxheader.mbxtype = MBXCOE;
      coel->coeheader.numberservice =
         htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
      coel->infoheader.opcode = COE_GETODLISTRESPONSE;
      /* number of objects request */
      if (etohs (coer->index) == 0x00)
      {
         coel->index = htoes ((uint16_t) 0x00);
         coel->infoheader.incomplete = 0;
         coel->infoheader.reserved = 0x00;
         coel->infoheader.fragmentsleft = htoes ((uint16_t) 0);
         MBXcontrol[0].state = MBXstate_idle;
         ESCvar.xoe = 0;
         ESCvar.frags = frags;
         ESCvar.fragsleft = frags - 1;
         p = &(coel->datatype);
         *p = htoes (entries);
         p++;
         *p = 0;
         p++;
         *p = 0;
         p++;
         *p = 0;
         p++;
         *p = 0;
         coel->mbxheader.length = htoes (0x08 + (5 << 1));
      }
      /* only return all objects */
      if (etohs (coer->index) == 0x01)
      {
         if (frags > 1)
         {
            coel->infoheader.incomplete = 1;
            ESCvar.xoe = MBXCOE + MBXODL;
            n = ODLISTSIZE >> 1;
         }
         else
         {
            coel->infoheader.incomplete = 0;
            MBXcontrol[0].state = MBXstate_idle;
            ESCvar.xoe = 0;
            n = entries;
         }
         coel->infoheader.reserved = 0x00;
         ESCvar.frags = frags;
         ESCvar.fragsleft = frags - 1;
         coel->infoheader.fragmentsleft = htoes (ESCvar.fragsleft);
         coel->index = htoes ((uint16_t) 0x01);

         p = &(coel->datatype);
         for (i = 0; i < n; i++)
         {
            *p = htoes (SDOobjects[i].index);
            p++;
         }

         coel->mbxheader.length = htoes (0x08 + (n << 1));
      }
      MBXcontrol[MBXout].state = MBXstate_outreq;
   }
}
/** Function for continuing sending left overs from previous requested
 * SDO Get OD List, validating the request and sending an response.
 *
 */
void SDO_getodlistcont (void)
{
   uint8_t MBXout;
   uint16_t i, n, s;
   uint16_t *p;
   _COEobjdesc *coel;

   MBXout = ESC_claimbuffer ();
   if (MBXout)
   {
      coel = (_COEobjdesc *) &MBX[MBXout * ESC_MBXSIZE];
      coel->mbxheader.mbxtype = MBXCOE;
      coel->coeheader.numberservice =
         htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
      coel->infoheader.opcode = COE_GETODLISTRESPONSE;
      s = (ESCvar.frags - ESCvar.fragsleft) * (ODLISTSIZE >> 1);
      if (ESCvar.fragsleft > 1)
      {
         coel->infoheader.incomplete = 1;
         n = s + (ODLISTSIZE >> 1);
      }
      else
      {
         coel->infoheader.incomplete = 0;
         MBXcontrol[0].state = MBXstate_idle;
         ESCvar.xoe = 0;
         n = ESCvar.entries;
      }
      coel->infoheader.reserved = 0x00;
      ESCvar.fragsleft--;
      coel->infoheader.fragmentsleft = htoes (ESCvar.fragsleft);
      /* pointer 2 bytes back to exclude index */
      p = &(coel->index);
      for (i = s; i < n; i++)
      {
         *p = htoes (SDOobjects[i].index);
         p++;
      }
      coel->mbxheader.length = htoes (0x06 + ((n - s) << 1));
      MBXcontrol[MBXout].state = MBXstate_outreq;
   }
}

/** Function for handling incoming requested SDO Get Object Description,
 * validating the request and sending an response. On error an
 * SDO Info Error will be sent.
 */
void SDO_getod (void)
{
   uint8_t MBXout;
   uint16_t index;
   int32_t nidx;
   uint8_t *d;
   const uint8_t *s;
   uint8_t n = 0;
   _COEobjdesc *coer, *coel;
   coer = (_COEobjdesc *) &MBX[0];
   index = etohs (coer->index);
   nidx = SDO_findobject (index);
   if (nidx >= 0)
   {
      MBXout = ESC_claimbuffer ();
      if (MBXout)
      {
         coel = (_COEobjdesc *) &MBX[MBXout * ESC_MBXSIZE];
         coel->mbxheader.mbxtype = MBXCOE;
         coel->coeheader.numberservice =
            htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
         coel->infoheader.opcode = COE_GETODRESPONSE;
         coel->infoheader.incomplete = 0;
         coel->infoheader.reserved = 0x00;
         coel->infoheader.fragmentsleft = htoes (0);
         coel->index = htoes (index);
         if (SDOobjects[nidx].objtype == OTYPE_VAR)
         {
            int32_t nsub = SDO_findsubindex (nidx, 0);
            const _objd *objd = SDOobjects[nidx].objdesc;
            coel->datatype = htoes ((objd + nsub)->datatype);
            coel->maxsub = SDOobjects[nidx].maxsub;
         }
         else if (SDOobjects[nidx].objtype == OTYPE_ARRAY)
         {
            int32_t nsub = SDO_findsubindex (nidx, 0);
            const _objd *objd = SDOobjects[nidx].objdesc;
            coel->datatype = htoes ((objd + nsub)->datatype);
            coel->maxsub = SDOobjects[nidx].objdesc->value;
         }
         else
         {
            coel->datatype = htoes (0);
            coel->maxsub = SDOobjects[nidx].objdesc->value;
         }
         coel->objectcode = SDOobjects[nidx].objtype;
         s = (uint8_t *) SDOobjects[nidx].name;
         d = (uint8_t *) &(coel->name);
         while (*s && (n < (ESC_MBXDSIZE - 0x0c)))
         {
            *d = *s;
            n++;
            s++;
            d++;
         }
         *d = *s;
         coel->mbxheader.length = htoes ((uint16_t) 0x0c + n);
         MBXcontrol[MBXout].state = MBXstate_outreq;
         MBXcontrol[0].state = MBXstate_idle;
         ESCvar.xoe = 0;
      }
   }
   else
   {
      SDO_infoerror (ABORT_NOOBJECT);
   }
}

/** Function for handling incoming requested SDO Get Entry Description,
 * validating the request and sending an response. On error an
 * SDO Info Error will be sent.
 */
void SDO_geted (void)
{
   uint8_t MBXout;
   uint16_t index;
   int32_t nidx, nsub;
   uint8_t subindex;
   uint8_t *d;
   const uint8_t *s;
   const _objd *objd;
   uint8_t n = 0;
   _COEentdesc *coer, *coel;
   coer = (_COEentdesc *) &MBX[0];
   index = etohs (coer->index);
   subindex = coer->subindex;
   nidx = SDO_findobject (index);
   if (nidx >= 0)
   {
      nsub = SDO_findsubindex (nidx, subindex);
      if (nsub >= 0)
      {
         objd = SDOobjects[nidx].objdesc;
         MBXout = ESC_claimbuffer ();
         if (MBXout)
         {
            coel = (_COEentdesc *) &MBX[MBXout * ESC_MBXSIZE];
            coel->mbxheader.mbxtype = MBXCOE;
            coel->coeheader.numberservice =
               htoes ((0 & 0x01f) | (COE_SDOINFORMATION << 12));
            coel->infoheader.opcode = COE_ENTRYDESCRIPTIONRESPONSE;
            coel->infoheader.incomplete = 0;
            coel->infoheader.reserved = 0x00;
            coel->infoheader.fragmentsleft = htoes ((uint16_t) 0);
            coel->index = htoes (index);
            coel->subindex = subindex;
            coel->valueinfo = COE_VALUEINFO_ACCESS +
               COE_VALUEINFO_OBJECT + COE_VALUEINFO_MAPPABLE;
            coel->datatype = htoes ((objd + nsub)->datatype);
            coel->bitlength = htoes ((objd + nsub)->bitlength);
            coel->access = htoes ((objd + nsub)->access);
            s = (uint8_t *) (objd + nsub)->name;
            d = (uint8_t *) &(coel->name);
            while (*s && (n < (ESC_MBXDSIZE - 0x10)))
            {
               *d = *s;
               n++;
               s++;
               d++;
            }
            *d = *s;
            coel->mbxheader.length = htoes ((uint16_t) 0x10 + n);
            MBXcontrol[MBXout].state = MBXstate_outreq;
            MBXcontrol[0].state = MBXstate_idle;
            ESCvar.xoe = 0;
         }
      }
      else
      {
         SDO_infoerror (ABORT_NOSUBINDEX);
      }
   }
   else
   {
      SDO_infoerror (ABORT_NOOBJECT);
   }
}

/** Main CoE function checking the status on current mailbox buffers carrying
 * data, distributing the mailboxes to appropriate CoE functions.
 * On Error an MBX_error or SDO Abort will be sent depending on error cause.
 */
void ESC_coeprocess (void)
{
   _MBXh *mbh;
   _COEsdo *coesdo;
   _COEobjdesc *coeobjdesc;
   uint8_t service;
   if (ESCvar.MBXrun == 0)
   {
      return;
   }
   if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
   {
      mbh = (_MBXh *) &MBX[0];
      if (mbh->mbxtype == MBXCOE)
      {
         if (etohs (mbh->length) < COE_MINIMUM_LENGTH)
         {
            MBX_error (MBXERR_INVALIDSIZE);
         }
         else
         {
            ESCvar.xoe = MBXCOE;
         }
      }
   }
   if ((ESCvar.xoe == (MBXCOE + MBXODL)) && (!ESCvar.mbxoutpost))
   {
      /* continue get OD list */
      SDO_getodlistcont ();
   }
   if (ESCvar.xoe == MBXCOE)
   {
      coesdo = (_COEsdo *) &MBX[0];
      coeobjdesc = (_COEobjdesc *) &MBX[0];
      service = etohs (coesdo->coeheader.numberservice) >> 12;
      /* initiate SDO upload request */
      if ((service == COE_SDOREQUEST)
          && (coesdo->command == COE_COMMAND_UPLOADREQUEST)
          && (etohs (coesdo->mbxheader.length) == 0x0a))
      {
         SDO_upload ();
      }
      /* SDO upload segment request */
      if ((service == COE_SDOREQUEST)
          && ((coesdo->command & 0xef) == COE_COMMAND_UPLOADSEGREQ)
          && (etohs (coesdo->mbxheader.length) == 0x0a)
          && (ESCvar.segmented == MBXSEU))
      {
         SDO_uploadsegment ();
      }
      /* initiate SDO download request */
      else
      {
         if ((service == COE_SDOREQUEST) && ((coesdo->command & 0xf1) == 0x21))
         {
            SDO_download ();
         }
         /* initiate SDO get OD list */
         else
         {
            if ((service == COE_SDOINFORMATION)
                && (coeobjdesc->infoheader.opcode == 0x01))
            {
               SDO_getodlist ();
            }
            /* initiate SDO get OD */
            else
            {
               if ((service == COE_SDOINFORMATION)
                   && (coeobjdesc->infoheader.opcode == 0x03))
               {
                  SDO_getod ();
               }
               /* initiate SDO get ED */
               else
               {
                  if ((service == COE_SDOINFORMATION)
                      && (coeobjdesc->infoheader.opcode == 0x05))
                  {
                     SDO_geted ();
                  }
                  else
                  {
                     /* COE not recognised above */
                     if (ESCvar.xoe == MBXCOE)
                     {
                        if (service == 0)
                        {
                           MBX_error (MBXERR_INVALIDHEADER);
                        }
                        else
                        {
                           SDO_abort (etohs (coesdo->index), coesdo->subindex, ABORT_UNSUPPORTED);
                        }
                        MBXcontrol[0].state = MBXstate_idle;
                        ESCvar.xoe = 0;
                     }
                  }
               }
            }
         }
      }
   }
}

/**
 * Get value from bitmap
 *
 * This function gets a value from a bitmap.
 *
 * @param[in] bitmap = bitmap containing value
 * @param[in] offset = start offset
 * @param[in] length = number of bits to get
 * @return bitslice value
 */
static uint64_t COE_bitsliceGet (uint64_t * bitmap, int offset, int length)
{
   const int word_offset = offset / 64;
   const int bit_offset = offset % 64;
   const uint64_t mask = (length == 64) ? UINT64_MAX : (1ULL << length) - 1;
   uint64_t w0;
   uint64_t w1 = 0;

   /* Get the least significant word */
   w0 = bitmap[word_offset];
   w0 = w0 >> bit_offset;

   /* Get the most significant word, if required */
   if (length + bit_offset > 64)
   {
      w1 = bitmap[word_offset + 1];
      w1 = w1 << (64 - bit_offset);
   }

   w0 = (w1 | w0);
   return (w0 & mask);
}

/**
 * Set value in bitmap
 *
 * This function sets a value in a bitmap.
 *
 * @param[in] bitmap = bitmap to contain value
 * @param[in] offset = start offset
 * @param[in] length = number of bits to set
 * @param[in] value  = value to set
 */
static void COE_bitsliceSet (uint64_t * bitmap, int offset, int length,
                             uint64_t value)
{
   const int word_offset = offset / 64;
   const int bit_offset = offset % 64;
   const uint64_t mask = (length == 64) ? UINT64_MAX : (1ULL << length) - 1;
   const uint64_t mask0 = mask << bit_offset;
   uint64_t v0 = value << bit_offset;
   uint64_t w0 = bitmap[word_offset];

   /* Set the least significant word */
   w0 = (w0 & ~mask0) | (v0 & mask0);
   bitmap[word_offset] = w0;

   /* Set the most significant word, if required */
   if (length + bit_offset > 64)
   {
      const uint64_t mask1 = mask >> (64 - bit_offset);
      uint64_t v1 = value >> (64 - bit_offset);
      uint64_t w1 = bitmap[word_offset + 1];

      w1 = (w1 & ~mask1) | (v1 & mask1);
      bitmap[word_offset + 1] = w1;
   }
}

/**
 * Get object value
 *
 * This function atomically gets an object value.
 *
 * @param[in] obj   = object description
 * @return object value
 */
static uint64_t COE_getValue (const _objd * obj)
{
   uint64_t value = 0;

   /* TODO: const data */

   switch(obj->datatype)
   {
   case DTYPE_BIT1:
   case DTYPE_BIT2:
   case DTYPE_BIT3:
   case DTYPE_BIT4:
   case DTYPE_BIT5:
   case DTYPE_BIT6:
   case DTYPE_BIT7:
   case DTYPE_BIT8:
   case DTYPE_BOOLEAN:
   case DTYPE_UNSIGNED8:
   case DTYPE_INTEGER8:
      value = *(uint8_t *)obj->data;
      break;

   case DTYPE_UNSIGNED16:
   case DTYPE_INTEGER16:
      value = *(uint16_t *)obj->data;
      break;

   case DTYPE_REAL32:
   case DTYPE_UNSIGNED32:
   case DTYPE_INTEGER32:
      value = *(uint32_t *)obj->data;
      break;

   case DTYPE_REAL64:
   case DTYPE_UNSIGNED64:
   case DTYPE_INTEGER64:
      /* FIXME: must be atomic */
      value = *(uint64_t *)obj->data;
      break;

   default:
      CC_ASSERT (0);
   }

   return value;
}

/**
 * Set object value
 *
 * This function atomically sets an object value.
 *
 * @param[in] obj   = object description
 * @param[in] value = new value
 */
static void COE_setValue (const _objd * obj, uint64_t value)
{
   switch(obj->datatype)
   {
   case DTYPE_BIT1:
   case DTYPE_BIT2:
   case DTYPE_BIT3:
   case DTYPE_BIT4:
   case DTYPE_BIT5:
   case DTYPE_BIT6:
   case DTYPE_BIT7:
   case DTYPE_BIT8:
   case DTYPE_BOOLEAN:
   case DTYPE_UNSIGNED8:
   case DTYPE_INTEGER8:
      *(uint8_t *)obj->data = value & UINT8_MAX;
      break;

   case DTYPE_UNSIGNED16:
   case DTYPE_INTEGER16:
      *(uint16_t *)obj->data = value & UINT16_MAX;
      break;

   case DTYPE_REAL32:
   case DTYPE_UNSIGNED32:
   case DTYPE_INTEGER32:
      *(uint32_t *)obj->data = value & UINT32_MAX;
      break;

   case DTYPE_REAL64:
   case DTYPE_UNSIGNED64:
   case DTYPE_INTEGER64:
      /* FIXME: must be atomic */
      *(uint64_t *)obj->data = value;
      break;

   default:
      DPRINT ("ignored\n");
      break;
   }
}

/**
 * Init default values for SDO objects
 */
void COE_initDefaultValues (void)
{
   int i;
   const _objd *objd;
   int n;
   uint8_t maxsub;

   for (n = 0; SDOobjects[n].index != 0xffff; n++)
   {
      objd = SDOobjects[n].objdesc;
      maxsub = SDOobjects[n].maxsub;

      i = 0;
      do
      {
         if (objd[i].data != NULL)
         {
            /* TODO: bitlength > 64 */
            COE_setValue (&objd[i], objd[i].value);
            DPRINT ("%04x:%02x = %x\n", SDOobjects[n].index, objd[i].subindex, objd[i].value);
         }
      } while (objd[i++].subindex < maxsub);
   }
}

/**
 * Pack process data
 *
 * This function reads mapped objects and constructs the process data
 * inputs (TXPDO).
 *
 * @param[in] buffer     = input process data
 * @param[in] nmappings  = number of mappings in sync manager
 * @param[in] mappings   = list of mapped objects in sync manager
 */
void COE_pdoPack (uint8_t * buffer, int nmappings, _SMmap * mappings)
{
   int ix;

   /* Check that buffer is aligned on 64-bit boundary */
   CC_ASSERT (((uintptr_t)buffer & 0x07) == 0);

   for (ix = 0; ix < nmappings; ix++)
   {
      const _objd * obj = mappings[ix].obj;
      uint16_t offset = mappings[ix].offset;

      if (obj != NULL)
      {
         if (obj->bitlength > 64)
         {
            memcpy (
               &buffer[BITS2BYTES (offset)],
               obj->data,
               BITS2BYTES (obj->bitlength)
            );
         }
         else
         {
            /* Atomically get object value */
            uint64_t value = COE_getValue (obj);
            COE_bitsliceSet (
               (uint64_t *)buffer,
               offset,
               obj->bitlength,
               value
            );
         }
      }
   }
}

/**
 * Unpack process data
 *
 * This function unpacks process data output (RXPDO) and writes to the
 * mapped objects.
 *
 * @param[in] buffer    = output process data
 * @param[in] nmappings = number of mappings in sync manager
 * @param[in] mappings  = list of mapped objects in sync manager
 */
void COE_pdoUnpack (uint8_t * buffer, int nmappings, _SMmap * mappings)
{
   int ix;

   /* Check that buffer is aligned on 64-bit boundary */
   CC_ASSERT (((uintptr_t)buffer & 0x07) == 0);

   for (ix = 0; ix < nmappings; ix++)
   {
      const _objd * obj = mappings[ix].obj;
      uint16_t offset = mappings[ix].offset;

      if (obj != NULL)
      {
         if (obj->bitlength > 64)
         {
            memcpy (
               obj->data,
               &buffer[BITS2BYTES (offset)],
               BITS2BYTES (obj->bitlength)
            );
         }
         else
         {
            /* Atomically set object value */
            uint64_t value = COE_bitsliceGet (
               (uint64_t *)buffer,
               offset,
               obj->bitlength
            );
            COE_setValue (obj, value);
         }
      }
   }
}

/**
 * Fetch max subindex
 *
 * This function fetches the value of subindex 0 (max subindex).
 *
 * @param[in] index = object index
 */
uint8_t COE_maxSub (uint16_t index)
{
   int nidx;
   uint8_t maxsub;

   nidx = SDO_findobject (index);
   if (nidx == -1)
      return 0;

   maxsub = OBJ_VALUE_FETCH (maxsub, SDOobjects[nidx].objdesc[0]);
   return maxsub;
}