#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <ethercat.h>
#include "ecmbind.h"

/** 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



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);
}


int ecmbind_enumerate_servicedescriptors(int Slave, cb_enum_indeces_t cb)
{
   ecx_contextt *context = &ecx_context;
   ec_SDOservicet *SDOp, *aSDOp;
   ec_mbxbuft MbxIn, MbxOut;
   int wkc;
   uint16 x, n, i, sp, offset;
   boolean stop;
   uint8 cnt;
   boolean First;

    if (cb == NULL)
        return -1;

   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 + (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 = 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;
               }

               /* extract indexes one by one */
               for (i = 0; i < n; i++)
                    cb(Slave, 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;
}

int ecmbind_read_objectdescription(int Slave, int index, cb_enum_sdo_descriptors_t cb)
{
   ecx_contextt *context = &ecx_context;
   ec_SDOservicet *SDOp, *aSDOp;
   int wkc;
   uint16  n;
   ec_mbxbuft MbxIn, MbxOut;
   uint8 cnt;
   char temp[128];

   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 + (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(index); /* 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 > sizeof(temp)-1)
                n = sizeof(temp)-1;
            strncpy(temp , (char *)&aSDOp->bdata[6], n);
            temp[n] = 0x00;

            cb(Slave, index, etohs(aSDOp->wdata[1]), aSDOp->bdata[5], aSDOp->bdata[4], temp);
         }
         /* got unexpected response from slave */
         else
         {
            if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */
            {
               ecx_SDOinfoerror(context, Slave, index, 0, etohl(aSDOp->ldata[0]));
            }
            else
            {
               ecx_packeterror(context, Slave, index, 0, 1); /* Unexpected frame returned */
            }
            wkc = 0;
         }
      }
   }

   return wkc;
}

int ecmbind_read_objectdescription_entry(uint16_t slave, uint16_t index, uint16_t sub, cb_enum_sdo_descriptors_t cb)
{
   ecx_contextt *context = &ecx_context;
   ec_SDOservicet *SDOp, *aSDOp;
   int wkc;
   int16 n;
   ec_mbxbuft MbxIn, MbxOut;
   uint8 cnt;
   char temp[128];

   wkc = 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(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 + (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] = sub;       /* 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))
         {
            n = (etohs(aSDOp->MbxHeader.length) - 16); /* length of string(name of object) */
            if (n > sizeof(temp)-1)
               n = sizeof(temp)-1;

            if (n < 0 )
               n = 0;

            strncpy(temp , (char *)&aSDOp->wdata[5], n);
            temp[n] = 0x00; /* string terminator */

            cb(slave, index, (ushort)etohs(aSDOp->wdata[2]), 0, sub, temp);

/*
            pOElist->ValueInfo[sub] = aSDOp->bdata[3];
            pOElist->DataType[sub] = etohs(aSDOp->wdata[2]);
            pOElist->BitLength[sub] = etohs(aSDOp->wdata[3]);
            pOElist->ObjAccess[sub] = etohs(aSDOp->wdata[4]);
*/
         }

         /* got unexpected response from slave */
         else
         {
            if (((aSDOp->Opcode & 0x7f) == ECT_SDOINFO_ERROR)) /* SDO info error received */
            {
               ecx_SDOinfoerror(context, slave, index, sub, etohl(aSDOp->ldata[0]));
            }
            else
            {
               ecx_packeterror(context, slave, index, sub, 1); /* Unexpected frame returned */
            }
            wkc = 0;
         }
      }
   }

   return wkc;
}