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

/** \file
 * \brief
 * Servo over EtherCAT (SoE) Module.
 */

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

#define EC_SOE_MAX_DRIVES 8

/** SoE (Servo over EtherCAT) mailbox structure */
PACKED_BEGIN
typedef struct PACKED
{
   ec_mbxheadert MbxHeader;
   uint8         opCode         :3;
   uint8         incomplete     :1;
   uint8         error          :1;
   uint8         driveNo        :3;
   uint8         elementflags;
   union
   {
      uint16     idn;
      uint16     fragmentsleft;
   };
} ec_SoEt;
PACKED_END

/** Report SoE error.
 *
 * @param[in]  context        = context struct
 * @param[in]  Slave      = Slave number
 * @param[in]  idn        = IDN that generated error
 * @param[in]  Error      = Error code, see EtherCAT documentation for list
 */
void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error)
{
   ec_errort Ec;

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

/** SoE read, blocking.
 *
 * The IDN object of the selected slave and DriveNo is read. If a 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]  driveNo       = Drive number in slave
 * @param[in]  elementflags  = Flags to select what properties of IDN are to be transferred.
 * @param[in]  idn           = IDN.
 * @param[in,out] psize      = Size in bytes of parameter buffer, returns bytes read from SoE.
 * @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_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
{
   ec_SoEt *SoEp, *aSoEp;
   int totalsize, framedatasize;
   int wkc;
   uint8 *bp;
   uint8 *mp;
   uint16 *errorcode;
   ec_mbxbuft MbxIn, MbxOut;
   uint8 cnt;
   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);
   aSoEp = (ec_SoEt *)&MbxIn;
   SoEp = (ec_SoEt *)&MbxOut;
   SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert));
   SoEp->MbxHeader.address = htoes(0x0000);
   SoEp->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;
   SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + MBX_HDR_SET_CNT(cnt); /* SoE */
   SoEp->opCode = ECT_SOE_READREQ;
   SoEp->incomplete = 0;
   SoEp->error = 0;
   SoEp->driveNo = driveNo;
   SoEp->elementflags = elementflags;
   SoEp->idn = htoes(idn);
   totalsize = 0;
   bp = p;
   mp = (uint8 *)&MbxIn + sizeof(ec_SoEt);
   NotLast = TRUE;
   /* send SoE request to slave */
   wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
   if (wkc > 0) /* succeeded to place mailbox in slave ? */
   {
      while (NotLast)
      {
         /* 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 SoE, ReadRes */
            if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
                (aSoEp->opCode == ECT_SOE_READRES) &&
                (aSoEp->error == 0) &&
                (aSoEp->driveNo == driveNo) &&
                (aSoEp->elementflags == elementflags))
            {
               framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt)  + sizeof(ec_mbxheadert);
               totalsize += framedatasize;
               /* Does parameter fit in parameter buffer ? */
               if (totalsize <= *psize)
               {
                  /* copy parameter data in parameter buffer */
                  memcpy(bp, mp, framedatasize);
                  /* increment buffer pointer */
                  bp += framedatasize;
               }
               else
               {
                  framedatasize -= totalsize - *psize;
                  totalsize = *psize;
                  /* copy parameter data in parameter buffer */
                  if (framedatasize > 0) memcpy(bp, mp, framedatasize);
               }

               if (!aSoEp->incomplete)
               {
                  NotLast = FALSE;
                  *psize = totalsize;
               }
            }
            /* other slave response */
            else
            {
               NotLast = FALSE;
               if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
                   (aSoEp->opCode == ECT_SOE_READRES) &&
                   (aSoEp->error == 1))
               {
                  mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
                  errorcode = (uint16 *)mp;
                  ecx_SoEerror(context, slave, idn, *errorcode);
               }
               else
               {
                  ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
               }
               wkc = 0;
            }
         }
         else
         {
            NotLast = FALSE;
            ecx_packeterror(context, slave, idn, 0, 4); /* no response */
         }
      }
   }
   return wkc;
}

/** SoE write, blocking.
 *
 * The IDN object of the selected slave and DriveNo is written. If a response
 * is larger than the mailbox size then the response is segmented.
 *
 * @param[in]  context        = context struct
 * @param[in]  slave         = Slave number
 * @param[in]  driveNo       = Drive number in slave
 * @param[in]  elementflags  = Flags to select what properties of IDN are to be transferred.
 * @param[in]  idn           = IDN.
 * @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_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
{
   ec_SoEt *SoEp, *aSoEp;
   int framedatasize, maxdata;
   int wkc;
   uint8 *mp;
   uint8 *hp;
   uint16 *errorcode;
   ec_mbxbuft MbxIn, MbxOut;
   uint8 cnt;
   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);
   aSoEp = (ec_SoEt *)&MbxIn;
   SoEp = (ec_SoEt *)&MbxOut;
   SoEp->MbxHeader.address = htoes(0x0000);
   SoEp->MbxHeader.priority = 0x00;
   SoEp->opCode = ECT_SOE_WRITEREQ;
   SoEp->error = 0;
   SoEp->driveNo = driveNo;
   SoEp->elementflags = elementflags;
   hp = p;
   mp = (uint8 *)&MbxOut + sizeof(ec_SoEt);
   maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt);
   NotLast = TRUE;
   while (NotLast)
   {
      framedatasize = psize;
      NotLast = FALSE;
      SoEp->idn = htoes(idn);
      SoEp->incomplete = 0;
      if (framedatasize > maxdata)
      {
         framedatasize = maxdata;  /*  segmented transfer needed  */
         NotLast = TRUE;
         SoEp->incomplete = 1;
         SoEp->fragmentsleft = (uint16)(psize / maxdata);
      }
      SoEp->MbxHeader.length = htoes((uint16)(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize));
      /* get new mailbox counter, used for session handle */
      cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
      context->slavelist[slave].mbx_cnt = cnt;
      SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + MBX_HDR_SET_CNT(cnt); /* SoE */
      /* copy parameter data to mailbox */
      memcpy(mp, hp, framedatasize);
      hp += framedatasize;
      psize -= framedatasize;
      /* send SoE request to slave */
      wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
      if (wkc > 0) /* succeeded to place mailbox in slave ? */
      {
         if (!NotLast || !ecx_mbxempty(context, slave, timeout))
         {
            /* 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 ? */
            {
               NotLast = FALSE;
               /* slave response should be SoE, WriteRes */
               if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
                   (aSoEp->opCode == ECT_SOE_WRITERES) &&
                   (aSoEp->error == 0) &&
                   (aSoEp->driveNo == driveNo) &&
                   (aSoEp->elementflags == elementflags))
               {
                  /* SoE write succeeded */
               }
               /* other slave response */
               else
               {
                  if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
                      (aSoEp->opCode == ECT_SOE_READRES) &&
                      (aSoEp->error == 1))
                  {
                     mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
                     errorcode = (uint16 *)mp;
                     ecx_SoEerror(context, slave, idn, *errorcode);
                  }
                  else
                  {
                     ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
                  }
                  wkc = 0;
               }
            }
            else
            {
               ecx_packeterror(context, slave, idn, 0, 4); /* no response */
            }
         }
      }
   }
   return wkc;
}

/** SoE read AT and MTD mapping.
 *
 * SoE has standard indexes defined for mapping. This function
 * tries to read them and collect a full input and output mapping size
 * of designated slave.
 *
 * @param[in]  context = context struct
 * @param[in]  slave   = Slave number
 * @param[out] Osize   = Size in bits of output mapping (MTD) found
 * @param[out] Isize   = Size in bits of input mapping (AT) found
 * @return >0 if mapping successful.
 */
int ecx_readIDNmap(ecx_contextt *context, uint16 slave, uint32 *Osize, uint32 *Isize)
{
   int retVal = 0;
   int   wkc;
   int psize;
   uint8 driveNr;
   uint16 entries, itemcount;
   ec_SoEmappingt     SoEmapping;
   ec_SoEattributet   SoEattribute;

   *Isize = 0;
   *Osize = 0;
   for(driveNr = 0; driveNr < EC_SOE_MAX_DRIVES; driveNr++)
   {
      psize = sizeof(SoEmapping);
      /* read output mapping via SoE */
      wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
      if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
      {
         /* command word (uint16) is always mapped but not in list */
         *Osize += 16;
         for (itemcount = 0 ; itemcount < entries ; itemcount++)
         {
            psize = sizeof(SoEattribute);
            /* read attribute of each IDN in mapping list */
            wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
            if ((wkc > 0) && (!SoEattribute.list))
            {
               /* length : 0 = 8bit, 1 = 16bit .... */
               *Osize += (int)8 << SoEattribute.length;
            }
         }
      }
      psize = sizeof(SoEmapping);
      /* read input mapping via SoE */
      wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
      if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
      {
         /* status word (uint16) is always mapped but not in list */
         *Isize += 16;
         for (itemcount = 0 ; itemcount < entries ; itemcount++)
         {
            psize = sizeof(SoEattribute);
            /* read attribute of each IDN in mapping list */
            wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
            if ((wkc > 0) && (!SoEattribute.list))
            {
               /* length : 0 = 8bit, 1 = 16bit .... */
               *Isize += (int)8 << SoEattribute.length;
            }
         }
      }
   }

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

#ifdef EC_VER1
int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
{
   return ecx_SoEread(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
}

int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
{
   return ecx_SoEwrite(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
}

int ec_readIDNmap(uint16 slave, uint32 *Osize, uint32 *Isize)
{
   return ecx_readIDNmap(&ecx_context, slave, Osize, Isize);
}
#endif