ethercatdc.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  * Distributed Clock EtherCAT functions.
00009  *
00010  */
00011 #include "oshw.h"
00012 #include "osal.h"
00013 #include "ethercattype.h"
00014 #include "ethercatbase.h"
00015 #include "ethercatmain.h"
00016 #include "ethercatdc.h"
00017 
00018 #define PORTM0 0x01
00019 #define PORTM1 0x02
00020 #define PORTM2 0x04
00021 #define PORTM3 0x08
00022 
00023 /** 1st sync pulse delay in ns here 100ms */
00024 #define SyncDelay       ((int32)100000000)
00025 
00026 /**
00027  * Set DC of slave to fire sync0 at CyclTime interval with CyclShift offset.
00028  *
00029  * @param[in]  context        = context struct
00030  * @param [in] slave            Slave number.
00031  * @param [in] act              TRUE = active, FALSE = deactivated
00032  * @param [in] CyclTime         Cycltime in ns.
00033  * @param [in] CyclShift        CyclShift in ns.
00034  */
00035 void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
00036 {
00037    uint8 h, RA;
00038    uint16 slaveh;
00039    int64 t, t1;
00040    int32 tc;
00041 
00042    slaveh = context->slavelist[slave].configadr;
00043    RA = 0;
00044 
00045    /* stop cyclic operation, ready for next trigger */
00046    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
00047    if (act)
00048    {
00049        RA = 1 + 2;    /* act cyclic operation and sync0, sync1 deactivated */
00050    }
00051    h = 0;
00052    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
00053    t1 = 0;
00054    (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
00055    t1 = etohll(t1);
00056 
00057    /* Calculate first trigger time, always a whole multiple of CyclTime rounded up
00058    plus the shifttime (can be negative)
00059    This insures best synchronization between slaves, slaves with the same CyclTime
00060    will sync at the same moment (you can use CyclShift to shift the sync) */
00061    if (CyclTime > 0)
00062    {
00063        t = ((t1 + SyncDelay) / CyclTime) * CyclTime + CyclTime + CyclShift;
00064    }
00065    else
00066    {
00067       t = t1 + SyncDelay + CyclShift;
00068       /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
00069    }
00070    t = htoell(t);
00071    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
00072    tc = htoel(CyclTime);
00073    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
00074    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
00075 
00076     // update ec_slave state
00077     context->slavelist[slave].DCactive = (uint8)act;
00078     context->slavelist[slave].DCshift = CyclShift;
00079     context->slavelist[slave].DCcycle = CyclTime;
00080 }
00081 
00082 /**
00083  * Set DC of slave to fire sync0 and sync1 at CyclTime interval with CyclShift offset.
00084  *
00085  * @param[in]  context        = context struct
00086  * @param [in] slave            Slave number.
00087  * @param [in] act              TRUE = active, FALSE = deactivated
00088  * @param [in] CyclTime0        Cycltime SYNC0 in ns.
00089  * @param [in] CyclTime1        Cycltime SYNC1 in ns. This time is a delta time in relation to
00090                                 the SYNC0 fire. If CylcTime1 = 0 then SYNC1 fires a the same time
00091                                 as SYNC0.
00092  * @param [in] CyclShift        CyclShift in ns.
00093  */
00094 void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift)
00095 {
00096    uint8 h, RA;
00097    uint16 slaveh;
00098    int64 t, t1;
00099    int32 tc;
00100    uint32 TrueCyclTime;
00101 
00102    /* Sync1 can be used as a multiple of Sync0, use true cycle time */
00103    TrueCyclTime = ((CyclTime1 / CyclTime0) + 1) * CyclTime0;
00104 
00105    slaveh = context->slavelist[slave].configadr;
00106    RA = 0;
00107 
00108    /* stop cyclic operation, ready for next trigger */
00109    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
00110    if (act)
00111    {
00112       RA = 1 + 2 + 4;    /* act cyclic operation and sync0 + sync1 */
00113    }
00114    h = 0;
00115    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
00116    t1 = 0;
00117    (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
00118    t1 = etohll(t1);
00119 
00120    /* Calculate first trigger time, always a whole multiple of TrueCyclTime rounded up
00121    plus the shifttime (can be negative)
00122    This insures best synchronization between slaves, slaves with the same CyclTime
00123    will sync at the same moment (you can use CyclShift to shift the sync) */
00124    if (CyclTime0 > 0)
00125    {
00126       t = ((t1 + SyncDelay) / TrueCyclTime) * TrueCyclTime + TrueCyclTime + CyclShift;
00127    }
00128    else
00129    {
00130       t = t1 + SyncDelay + CyclShift;
00131       /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
00132    }
00133    t = htoell(t);
00134    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
00135    tc = htoel(CyclTime0);
00136    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
00137    tc = htoel(CyclTime1);
00138    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE1, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC1 cycle time */
00139    (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
00140 
00141     // update ec_slave state
00142     context->slavelist[slave].DCactive = (uint8)act;
00143     context->slavelist[slave].DCshift = CyclShift;
00144     context->slavelist[slave].DCcycle = CyclTime0;
00145 }
00146 
00147 /* latched port time of slave */
00148 static int32 ecx_porttime(ecx_contextt *context, uint16 slave, uint8 port)
00149 {
00150    int32 ts;
00151    switch (port)
00152    {
00153       case 0:
00154          ts = context->slavelist[slave].DCrtA;
00155          break;
00156       case 1:
00157          ts = context->slavelist[slave].DCrtB;
00158          break;
00159       case 2:
00160          ts = context->slavelist[slave].DCrtC;
00161          break;
00162       case 3:
00163          ts = context->slavelist[slave].DCrtD;
00164          break;
00165       default:
00166          ts = 0;
00167          break;
00168    }
00169    return ts;
00170 }
00171 
00172 /* calculate previous active port of a slave */
00173 static uint8 ecx_prevport(ecx_contextt *context, uint16 slave, uint8 port)
00174 {
00175    uint8 pport = port;
00176    uint8 aport = context->slavelist[slave].activeports;
00177    switch(port)
00178    {
00179       case 0:
00180          if(aport & PORTM2)
00181             pport = 2;
00182          else if (aport & PORTM1)
00183             pport = 1;
00184          else if (aport & PORTM3)
00185             pport = 3;
00186          break;
00187       case 1:
00188          if(aport & PORTM3)
00189             pport = 3;
00190          else if (aport & PORTM0)
00191             pport = 0;
00192          else if (aport & PORTM2)
00193             pport = 2;
00194          break;
00195       case 2:
00196          if(aport & PORTM1)
00197             pport = 1;
00198          else if (aport & PORTM3)
00199             pport = 3;
00200          else if (aport & PORTM0)
00201             pport = 0;
00202          break;
00203       case 3:
00204          if(aport & PORTM0)
00205             pport = 0;
00206          else if (aport & PORTM2)
00207             pport = 2;
00208          else if (aport & PORTM1)
00209             pport = 1;
00210          break;
00211    }
00212    return pport;
00213 }
00214 
00215 /* search unconsumed ports in parent, consume and return first open port */
00216 static uint8 ecx_parentport(ecx_contextt *context, uint16 parent)
00217 {
00218    uint8 parentport = 0;
00219    uint8 b;
00220    /* search order is important, here 3 - 1 - 2 - 0 */
00221    b = context->slavelist[parent].consumedports;
00222    if (b & PORTM3)
00223    {
00224       parentport = 3;
00225       b &= (uint8)~PORTM3;
00226    }
00227    else if (b & PORTM1)
00228    {
00229       parentport = 1;
00230       b &= (uint8)~PORTM1;
00231    }
00232    else if (b & PORTM2)
00233    {
00234       parentport = 2;
00235       b &= (uint8)~PORTM2;
00236    }
00237    else if (b & PORTM0)
00238    {
00239       parentport = 0;
00240       b &= (uint8)~PORTM0;
00241    }
00242    context->slavelist[parent].consumedports = b;
00243    return parentport;
00244 }
00245 
00246 /**
00247  * Locate DC slaves, measure propagation delays.
00248  *
00249  * @param[in]  context        = context struct
00250  * @return boolean if slaves are found with DC
00251  */
00252 boolean ecx_configdc(ecx_contextt *context)
00253 {
00254    uint16 i, slaveh, parent, child;
00255    uint16 parenthold = 0;
00256    uint16 prevDCslave = 0;
00257    int32 ht, dt1, dt2, dt3;
00258    int64 hrt;
00259    uint8 entryport;
00260    int8 nlist;
00261    int8 plist[4];
00262    int32 tlist[4];
00263    ec_timet mastertime;
00264    uint64 mastertime64;
00265 
00266    context->slavelist[0].hasdc = FALSE;
00267    context->grouplist[0].hasdc = FALSE;
00268    ht = 0;
00269 
00270    ecx_BWR(context->port, 0, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET);  /* latch DCrecvTimeA of all slaves */
00271    mastertime = osal_current_time();
00272    mastertime.sec -= 946684800UL;  /* EtherCAT uses 2000-01-01 as epoch start instead of 1970-01-01 */
00273    mastertime64 = (((uint64)mastertime.sec * 1000000) + (uint64)mastertime.usec) * 1000;
00274    for (i = 1; i <= *(context->slavecount); i++)
00275    {
00276       context->slavelist[i].consumedports = context->slavelist[i].activeports;
00277       if (context->slavelist[i].hasdc)
00278       {
00279          if (!context->slavelist[0].hasdc)
00280          {
00281             context->slavelist[0].hasdc = TRUE;
00282             context->slavelist[0].DCnext = i;
00283             context->slavelist[i].DCprevious = 0;
00284             context->grouplist[context->slavelist[i].group].hasdc = TRUE;
00285             context->grouplist[context->slavelist[i].group].DCnext = i;
00286          }
00287          else
00288          {
00289             context->slavelist[prevDCslave].DCnext = i;
00290             context->slavelist[i].DCprevious = prevDCslave;
00291          }
00292          /* this branch has DC slave so remove parenthold */
00293          parenthold = 0;
00294          prevDCslave = i;
00295          slaveh = context->slavelist[i].configadr;
00296          (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET);
00297          context->slavelist[i].DCrtA = etohl(ht);
00298          /* 64bit latched DCrecvTimeA of each specific slave */
00299          (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSOF, sizeof(hrt), &hrt, EC_TIMEOUTRET);
00300          /* use it as offset in order to set local time around 0 + mastertime */
00301          hrt = htoell(-etohll(hrt) + mastertime64);
00302          /* save it in the offset register */
00303          (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSOFFSET, sizeof(hrt), &hrt, EC_TIMEOUTRET);
00304          (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME1, sizeof(ht), &ht, EC_TIMEOUTRET);
00305          context->slavelist[i].DCrtB = etohl(ht);
00306          (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME2, sizeof(ht), &ht, EC_TIMEOUTRET);
00307          context->slavelist[i].DCrtC = etohl(ht);
00308          (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME3, sizeof(ht), &ht, EC_TIMEOUTRET);
00309          context->slavelist[i].DCrtD = etohl(ht);
00310 
00311          /* make list of active ports and their time stamps */
00312          nlist = 0;
00313          if (context->slavelist[i].activeports & PORTM0)
00314          {
00315             plist[nlist] = 0;
00316             tlist[nlist] = context->slavelist[i].DCrtA;
00317             nlist++;
00318          }
00319          if (context->slavelist[i].activeports & PORTM3)
00320          {
00321             plist[nlist] = 3;
00322             tlist[nlist] = context->slavelist[i].DCrtD;
00323             nlist++;
00324          }
00325          if (context->slavelist[i].activeports & PORTM1)
00326          {
00327             plist[nlist] = 1;
00328             tlist[nlist] = context->slavelist[i].DCrtB;
00329             nlist++;
00330          }
00331          if (context->slavelist[i].activeports & PORTM2)
00332          {
00333             plist[nlist] = 2;
00334             tlist[nlist] = context->slavelist[i].DCrtC;
00335             nlist++;
00336          }
00337          /* entryport is port with the lowest timestamp */
00338          entryport = 0;
00339          if((nlist > 1) && (tlist[1] < tlist[entryport]))
00340          {
00341             entryport = 1;
00342          }
00343          if((nlist > 2) && (tlist[2] < tlist[entryport]))
00344          {
00345             entryport = 2;
00346          }
00347          if((nlist > 3) && (tlist[3] < tlist[entryport]))
00348          {
00349             entryport = 3;
00350          }
00351          entryport = plist[entryport];
00352          context->slavelist[i].entryport = entryport;
00353          /* consume entryport from activeports */
00354          context->slavelist[i].consumedports &= (uint8)~(1 << entryport);
00355 
00356          /* finding DC parent of current */
00357          parent = i;
00358          do
00359          {
00360             child = parent;
00361             parent = context->slavelist[parent].parent;
00362          }
00363          while (!((parent == 0) || (context->slavelist[parent].hasdc)));
00364          /* only calculate propagation delay if slave is not the first */
00365          if (parent > 0)
00366          {
00367             /* find port on parent this slave is connected to */
00368             context->slavelist[i].parentport = ecx_parentport(context, parent);
00369             if (context->slavelist[parent].topology == 1)
00370             {
00371                context->slavelist[i].parentport = context->slavelist[parent].entryport;
00372             }
00373 
00374             dt1 = 0;
00375             dt2 = 0;
00376             /* delta time of (parentport - 1) - parentport */
00377             /* note: order of ports is 0 - 3 - 1 -2 */
00378             /* non active ports are skipped */
00379             dt3 = ecx_porttime(context, parent, context->slavelist[i].parentport) -
00380                   ecx_porttime(context, parent,
00381                     ecx_prevport(context, parent, context->slavelist[i].parentport));
00382             /* current slave has children */
00383             /* those children's delays need to be subtracted */
00384             if (context->slavelist[i].topology > 1)
00385             {
00386                dt1 = ecx_porttime(context, i,
00387                         ecx_prevport(context, i, context->slavelist[i].entryport)) -
00388                      ecx_porttime(context, i, context->slavelist[i].entryport);
00389             }
00390             /* we are only interested in positive difference */
00391             if (dt1 > dt3) dt1 = -dt1;
00392             /* current slave is not the first child of parent */
00393             /* previous child's delays need to be added */
00394             if ((child - parent) > 1)
00395             {
00396                dt2 = ecx_porttime(context, parent,
00397                         ecx_prevport(context, parent, context->slavelist[i].parentport)) -
00398                      ecx_porttime(context, parent, context->slavelist[parent].entryport);
00399             }
00400             if (dt2 < 0) dt2 = -dt2;
00401 
00402             /* calculate current slave delay from delta times */
00403             /* assumption : forward delay equals return delay */
00404             context->slavelist[i].pdelay = ((dt3 - dt1) / 2) + dt2 +
00405                context->slavelist[parent].pdelay;
00406             ht = htoel(context->slavelist[i].pdelay);
00407             /* write propagation delay*/
00408             (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSDELAY, sizeof(ht), &ht, EC_TIMEOUTRET);
00409          }
00410       }
00411       else
00412       {
00413          context->slavelist[i].DCrtA = 0;
00414          context->slavelist[i].DCrtB = 0;
00415          context->slavelist[i].DCrtC = 0;
00416          context->slavelist[i].DCrtD = 0;
00417          parent = context->slavelist[i].parent;
00418          /* if non DC slave found on first position on branch hold root parent */
00419          if ( (parent > 0) && (context->slavelist[parent].topology > 2))
00420             parenthold = parent;
00421          /* if branch has no DC slaves consume port on root parent */
00422          if ( parenthold && (context->slavelist[i].topology == 1))
00423          {
00424             ecx_parentport(context, parenthold);
00425             parenthold = 0;
00426          }
00427       }
00428    }
00429 
00430    return context->slavelist[0].hasdc;
00431 }
00432 
00433 #ifdef EC_VER1
00434 void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
00435 {
00436    ecx_dcsync0(&ecx_context, slave, act, CyclTime, CyclShift);
00437 }
00438 
00439 void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift)
00440 {
00441    ecx_dcsync01(&ecx_context, slave, act, CyclTime0, CyclTime1, CyclShift);
00442 }
00443 
00444 boolean ec_configdc(void)
00445 {
00446    return ecx_configdc(&ecx_context);
00447 }
00448 #endif