ethercatsoe.c

00001 /*
00002  * Licensed under the GNU General Public License version 2 with exceptions. See
00003  * LICENSE file in the project root for full license information
00004  */
00005 
00006 /** \file
00007  * \brief
00008  * Servo over EtherCAT (SoE) Module.
00009  */
00010 
00011 #include <stdio.h>
00012 #include <string.h>
00013 #include "osal.h"
00014 #include "oshw.h"
00015 #include "ethercattype.h"
00016 #include "ethercatbase.h"
00017 #include "ethercatmain.h"
00018 #include "ethercatsoe.h"
00019 
00020 #define EC_SOE_MAX_DRIVES 8
00021 
00022 /** SoE (Servo over EtherCAT) mailbox structure */
00023 PACKED_BEGIN
00024 typedef struct PACKED
00025 {
00026    ec_mbxheadert MbxHeader;
00027    uint8         opCode         :3;
00028    uint8         incomplete     :1;
00029    uint8         error          :1;
00030    uint8         driveNo        :3;
00031    uint8         elementflags;
00032    union
00033    {
00034       uint16     idn;
00035       uint16     fragmentsleft;
00036    };
00037 } ec_SoEt;
00038 PACKED_END
00039 
00040 /** Report SoE error.
00041  *
00042  * @param[in]  context        = context struct
00043  * @param[in]  Slave      = Slave number
00044  * @param[in]  idn        = IDN that generated error
00045  * @param[in]  Error      = Error code, see EtherCAT documentation for list
00046  */
00047 void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error)
00048 {
00049    ec_errort Ec;
00050 
00051    memset(&Ec, 0, sizeof(Ec));
00052    Ec.Time = osal_current_time();
00053    Ec.Slave = Slave;
00054    Ec.Index = idn;
00055    Ec.SubIdx = 0;
00056    *(context->ecaterror) = TRUE;
00057    Ec.Etype = EC_ERR_TYPE_SOE_ERROR;
00058    Ec.ErrorCode = Error;
00059    ecx_pusherror(context, &Ec);
00060 }
00061 
00062 /** SoE read, blocking.
00063  *
00064  * The IDN object of the selected slave and DriveNo is read. If a response
00065  * is larger than the mailbox size then the response is segmented. The function
00066  * will combine all segments and copy them to the parameter buffer.
00067  *
00068  * @param[in]  context        = context struct
00069  * @param[in]  slave         = Slave number
00070  * @param[in]  driveNo       = Drive number in slave
00071  * @param[in]  elementflags  = Flags to select what properties of IDN are to be transferred.
00072  * @param[in]  idn           = IDN.
00073  * @param[in,out] psize      = Size in bytes of parameter buffer, returns bytes read from SoE.
00074  * @param[out] p             = Pointer to parameter buffer
00075  * @param[in]  timeout       = Timeout in us, standard is EC_TIMEOUTRXM
00076  * @return Workcounter from last slave response
00077  */
00078 int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
00079 {
00080    ec_SoEt *SoEp, *aSoEp;
00081    uint16 totalsize, framedatasize;
00082    int wkc;
00083    uint8 *bp;
00084    uint8 *mp;
00085    uint16 *errorcode;
00086    ec_mbxbuft MbxIn, MbxOut;
00087    uint8 cnt;
00088    boolean NotLast;
00089 
00090    ec_clearmbx(&MbxIn);
00091    /* Empty slave out mailbox if something is in. Timeout set to 0 */
00092    wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
00093    ec_clearmbx(&MbxOut);
00094    aSoEp = (ec_SoEt *)&MbxIn;
00095    SoEp = (ec_SoEt *)&MbxOut;
00096    SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert));
00097    SoEp->MbxHeader.address = htoes(0x0000);
00098    SoEp->MbxHeader.priority = 0x00;
00099    /* get new mailbox count value, used as session handle */
00100    cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00101    context->slavelist[slave].mbx_cnt = cnt;
00102    SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
00103    SoEp->opCode = ECT_SOE_READREQ;
00104    SoEp->incomplete = 0;
00105    SoEp->error = 0;
00106    SoEp->driveNo = driveNo;
00107    SoEp->elementflags = elementflags;
00108    SoEp->idn = htoes(idn);
00109    totalsize = 0;
00110    bp = p;
00111    mp = (uint8 *)&MbxIn + sizeof(ec_SoEt);
00112    NotLast = TRUE;
00113    /* send SoE request to slave */
00114    wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00115    if (wkc > 0) /* succeeded to place mailbox in slave ? */
00116    {
00117       while (NotLast)
00118       {
00119          /* clean mailboxbuffer */
00120          ec_clearmbx(&MbxIn);
00121          /* read slave response */
00122          wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00123          if (wkc > 0) /* succeeded to read slave response ? */
00124          {
00125             /* slave response should be SoE, ReadRes */
00126             if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
00127                 (aSoEp->opCode == ECT_SOE_READRES) &&
00128                 (aSoEp->error == 0) &&
00129                 (aSoEp->driveNo == driveNo) &&
00130                 (aSoEp->elementflags == elementflags))
00131             {
00132                framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt)  + sizeof(ec_mbxheadert);
00133                totalsize += framedatasize;
00134                /* Does parameter fit in parameter buffer ? */
00135                if (totalsize <= *psize)
00136                {
00137                   /* copy parameter data in parameter buffer */
00138                   memcpy(bp, mp, framedatasize);
00139                   /* increment buffer pointer */
00140                   bp += framedatasize;
00141                }
00142                else
00143                {
00144                   framedatasize -= totalsize - *psize;
00145                   totalsize = *psize;
00146                   /* copy parameter data in parameter buffer */
00147                   if (framedatasize > 0) memcpy(bp, mp, framedatasize);
00148                }
00149 
00150                if (!aSoEp->incomplete)
00151                {
00152                   NotLast = FALSE;
00153                   *psize = totalsize;
00154                }
00155             }
00156             /* other slave response */
00157             else
00158             {
00159                NotLast = FALSE;
00160                if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
00161                    (aSoEp->opCode == ECT_SOE_READRES) &&
00162                    (aSoEp->error == 1))
00163                {
00164                   mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
00165                   errorcode = (uint16 *)mp;
00166                   ecx_SoEerror(context, slave, idn, *errorcode);
00167                }
00168                else
00169                {
00170                   ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
00171                }
00172                wkc = 0;
00173             }
00174          }
00175          else
00176          {
00177             NotLast = FALSE;
00178             ecx_packeterror(context, slave, idn, 0, 4); /* no response */
00179          }
00180       }
00181    }
00182    return wkc;
00183 }
00184 
00185 /** SoE write, blocking.
00186  *
00187  * The IDN object of the selected slave and DriveNo is written. If a response
00188  * is larger than the mailbox size then the response is segmented.
00189  *
00190  * @param[in]  context        = context struct
00191  * @param[in]  slave         = Slave number
00192  * @param[in]  driveNo       = Drive number in slave
00193  * @param[in]  elementflags  = Flags to select what properties of IDN are to be transferred.
00194  * @param[in]  idn           = IDN.
00195  * @param[in]  psize         = Size in bytes of parameter buffer.
00196  * @param[out] p             = Pointer to parameter buffer
00197  * @param[in]  timeout       = Timeout in us, standard is EC_TIMEOUTRXM
00198  * @return Workcounter from last slave response
00199  */
00200 int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
00201 {
00202    ec_SoEt *SoEp, *aSoEp;
00203    uint16 framedatasize, maxdata;
00204    int wkc;
00205    uint8 *mp;
00206    uint8 *hp;
00207    uint16 *errorcode;
00208    ec_mbxbuft MbxIn, MbxOut;
00209    uint8 cnt;
00210    boolean NotLast;
00211 
00212    ec_clearmbx(&MbxIn);
00213    /* Empty slave out mailbox if something is in. Timeout set to 0 */
00214    wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0);
00215    ec_clearmbx(&MbxOut);
00216    aSoEp = (ec_SoEt *)&MbxIn;
00217    SoEp = (ec_SoEt *)&MbxOut;
00218    SoEp->MbxHeader.address = htoes(0x0000);
00219    SoEp->MbxHeader.priority = 0x00;
00220    SoEp->opCode = ECT_SOE_WRITEREQ;
00221    SoEp->error = 0;
00222    SoEp->driveNo = driveNo;
00223    SoEp->elementflags = elementflags;
00224    hp = p;
00225    mp = (uint8 *)&MbxOut + sizeof(ec_SoEt);
00226    maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt);
00227    NotLast = TRUE;
00228    while (NotLast)
00229    {
00230       framedatasize = psize;
00231       NotLast = FALSE;
00232       SoEp->idn = htoes(idn);
00233       SoEp->incomplete = 0;
00234       if (framedatasize > maxdata)
00235       {
00236          framedatasize = maxdata;  /*  segmented transfer needed  */
00237          NotLast = TRUE;
00238          SoEp->incomplete = 1;
00239          SoEp->fragmentsleft = psize / maxdata;
00240       }
00241       SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize);
00242       /* get new mailbox counter, used for session handle */
00243       cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt);
00244       context->slavelist[slave].mbx_cnt = cnt;
00245       SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */
00246       /* copy parameter data to mailbox */
00247       memcpy(mp, hp, framedatasize);
00248       hp += framedatasize;
00249       psize -= framedatasize;
00250       /* send SoE request to slave */
00251       wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM);
00252       if (wkc > 0) /* succeeded to place mailbox in slave ? */
00253       {
00254          if (!NotLast || !ecx_mbxempty(context, slave, timeout))
00255          {
00256             /* clean mailboxbuffer */
00257             ec_clearmbx(&MbxIn);
00258             /* read slave response */
00259             wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout);
00260             if (wkc > 0) /* succeeded to read slave response ? */
00261             {
00262                NotLast = FALSE;
00263                /* slave response should be SoE, WriteRes */
00264                if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
00265                    (aSoEp->opCode == ECT_SOE_WRITERES) &&
00266                    (aSoEp->error == 0) &&
00267                    (aSoEp->driveNo == driveNo) &&
00268                    (aSoEp->elementflags == elementflags))
00269                {
00270                   /* SoE write succeeded */
00271                }
00272                /* other slave response */
00273                else
00274                {
00275                   if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) &&
00276                       (aSoEp->opCode == ECT_SOE_READRES) &&
00277                       (aSoEp->error == 1))
00278                   {
00279                      mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16));
00280                      errorcode = (uint16 *)mp;
00281                      ecx_SoEerror(context, slave, idn, *errorcode);
00282                   }
00283                   else
00284                   {
00285                      ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */
00286                   }
00287                   wkc = 0;
00288                }
00289             }
00290             else
00291             {
00292                ecx_packeterror(context, slave, idn, 0, 4); /* no response */
00293             }
00294          }
00295       }
00296    }
00297    return wkc;
00298 }
00299 
00300 /** SoE read AT and MTD mapping.
00301  *
00302  * SoE has standard indexes defined for mapping. This function
00303  * tries to read them and collect a full input and output mapping size
00304  * of designated slave.
00305  *
00306  * @param[in]  context = context struct
00307  * @param[in]  slave   = Slave number
00308  * @param[out] Osize   = Size in bits of output mapping (MTD) found
00309  * @param[out] Isize   = Size in bits of input mapping (AT) found
00310  * @return >0 if mapping successful.
00311  */
00312 int ecx_readIDNmap(ecx_contextt *context, uint16 slave, int *Osize, int *Isize)
00313 {
00314    int retVal = 0;
00315    int   wkc;
00316    int psize;
00317    int driveNr;
00318    uint16 entries, itemcount;
00319    ec_SoEmappingt     SoEmapping;
00320    ec_SoEattributet   SoEattribute;
00321 
00322    *Isize = 0;
00323    *Osize = 0;
00324    for(driveNr = 0; driveNr < EC_SOE_MAX_DRIVES; driveNr++)
00325    {
00326       psize = sizeof(SoEmapping);
00327       /* read output mapping via SoE */
00328       wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
00329       if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
00330       {
00331          /* command word (uint16) is always mapped but not in list */
00332          *Osize = 16;
00333          for (itemcount = 0 ; itemcount < entries ; itemcount++)
00334          {
00335             psize = sizeof(SoEattribute);
00336             /* read attribute of each IDN in mapping list */
00337             wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
00338             if ((wkc > 0) && (!SoEattribute.list))
00339             {
00340                /* length : 0 = 8bit, 1 = 16bit .... */
00341                *Osize += (int)8 << SoEattribute.length;
00342             }
00343          }
00344       }
00345       psize = sizeof(SoEmapping);
00346       /* read input mapping via SoE */
00347       wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM);
00348       if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING))
00349       {
00350          /* status word (uint16) is always mapped but not in list */
00351          *Isize = 16;
00352          for (itemcount = 0 ; itemcount < entries ; itemcount++)
00353          {
00354             psize = sizeof(SoEattribute);
00355             /* read attribute of each IDN in mapping list */
00356             wkc = ecx_SoEread(context, slave, driveNr, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM);
00357             if ((wkc > 0) && (!SoEattribute.list))
00358             {
00359                /* length : 0 = 8bit, 1 = 16bit .... */
00360                *Isize += (int)8 << SoEattribute.length;
00361             }
00362          }
00363       }
00364    }
00365 
00366    /* found some I/O bits ? */
00367    if ((*Isize > 0) || (*Osize > 0))
00368    {
00369       retVal = 1;
00370    }
00371    return retVal;
00372 }
00373 
00374 #ifdef EC_VER1
00375 int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout)
00376 {
00377    return ecx_SoEread(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
00378 }
00379 
00380 int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout)
00381 {
00382    return ecx_SoEwrite(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout);
00383 }
00384 
00385 int ec_readIDNmap(uint16 slave, int *Osize, int *Isize)
00386 {
00387    return ecx_readIDNmap(&ecx_context, slave, Osize, Isize);
00388 }
00389 #endif