/**
* Set DC of slave to fire sync0 and sync1 at CyclTime interval with CyclShift offset.
*
* @param[in] context = context struct
* @param [in] slave Slave number.
* @param [in] act TRUE = active, FALSE = deactivated
* @param [in] CyclTime0 Cycltime SYNC0 in ns.
* @param [in] CyclTime1 Cycltime SYNC1 in ns. This time is a delta time in relation to
the SYNC0 fire. If CylcTime1 = 0 then SYNC1 fires a the same time
as SYNC0.
* @param [in] CyclShift CyclShift in ns.
*/
void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift)
{
uint8 h, RA;
uint16 slaveh;
int64 t, t1;
int32 tc;
uint32 TrueCyclTime;
/* Sync1 can be used as a multiple of Sync0, use true cycle time */
TrueCyclTime = ((CyclTime1 / CyclTime0) + 1) * CyclTime0;
slaveh = context->slavelist[slave].configadr;
RA = 0;
/* stop cyclic operation, ready for next trigger */
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
if (act)
{
RA = 1 + 2 + 4; /* act cyclic operation and sync0 + sync1 */
}
h = 0;
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
t1 = 0;
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
t1 = etohll(t1);
/* Calculate first trigger time, always a whole multiple of TrueCyclTime rounded up
plus the shifttime (can be negative)
This insures best sychronisation between slaves, slaves with the same CyclTime
will sync at the same moment (you can use CyclShift to shift the sync) */
if (CyclTime0 > 0)
{
t = ((t1 + SyncDelay) / TrueCyclTime) * TrueCyclTime + TrueCyclTime + CyclShift;
}
else
{
t = t1 + SyncDelay + CyclShift;
/* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
}
t = htoell(t);
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
tc = htoel(CyclTime0);
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
tc = htoel(CyclTime1);
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE1, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC1 cycle time */
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
// update ec_slave state
context->slavelist[slave].DCactive = (uint8)act;
context->slavelist[slave].DCshift = CyclShift;
context->slavelist[slave].DCcycle = CyclTime0;
}
/**
* Set DC of slave to fire sync0 at CyclTime interval with CyclShift offset.
*
* @param[in] context = context struct
* @param [in] slave Slave number.
* @param [in] act TRUE = active, FALSE = deactivated
* @param [in] CyclTime Cycltime in ns.
* @param [in] CyclShift CyclShift in ns.
*/
void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift)
{
uint8 h, RA;
uint16 wc, slaveh;
int64 t, t1;
int32 tc;
slaveh = context->slavelist[slave].configadr;
RA = 0;
/* stop cyclic operation, ready for next trigger */
wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
if (act)
{
RA = 1 + 2; /* act cyclic operation and sync0, sync1 deactivated */
}
h = 0;
wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
wc = ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
t1 = etohll(t1);
/* Calculate first trigger time, always a whole multiple of CyclTime rounded up
plus the shifttime (can be negative)
This insures best sychronisation between slaves, slaves with the same CyclTime
will sync at the same moment (you can use CyclShift to shift the sync) */
if (CyclTime > 0)
{
t = ((t1 + SyncDelay) / CyclTime) * CyclTime + CyclTime + CyclShift;
}
else
{
t = t1 + SyncDelay + CyclShift;
/* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
}
t = htoell(t);
wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
tc = htoel(CyclTime);
wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
}
/** Enumerate and init all slaves.
*
* @param[in] context = context struct
* @param[in] usetable = TRUE when using configtable to init slaves, FALSE otherwise
* @return Workcounter of slave discover datagram = number of slaves found
*/
int ecx_config_init(ecx_contextt *context, uint8 usetable)
{
uint16 w, slave, ADPh, configadr, ssigen;
uint16 topology, estat;
int16 topoc, slavec, aliasadr;
uint8 b,h;
uint8 zbuf[64];
uint8 SMc;
uint32 eedat;
int wkc, cindex, nSM, lp;
EC_PRINT("ec_config_init %d\n",usetable);
*(context->slavecount) = 0;
/* clean ec_slave array */
memset(context->slavelist, 0x00, sizeof(ec_slavet) * context->maxslave);
memset(&zbuf, 0x00, sizeof(zbuf));
memset(context->grouplist, 0x00, sizeof(ec_groupt) * context->maxgroup);
/* clear slave eeprom cache */
ecx_siigetbyte(context, 0, EC_MAXEEPBUF);
for(lp = 0; lp < context->maxgroup; lp++)
{
context->grouplist[lp].logstartaddr = lp << 16; /* default start address per group entry */
}
/* make special pre-init register writes to enable MAC[1] local administered bit *
* setting for old netX100 slaves */
b = 0x00;
ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS, sizeof(b), &b, EC_TIMEOUTRET3); /* Ignore Alias register */
b = EC_STATE_INIT | EC_STATE_ACK;
ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
/* netX100 should now be happy */
wkc = ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
printf("wkc = %d\n",wkc);
w = 0x0000;
wkc = ecx_BRD(context->port, 0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE); /* detect number of slaves */
if (wkc > 0)
{
*(context->slavecount) = wkc;
b = 0x00;
ecx_BWR(context->port, 0x0000, ECT_REG_DLPORT, sizeof(b), &b, EC_TIMEOUTRET3); /* deact loop manual */
w = htoes(0x0004);
ecx_BWR(context->port, 0x0000, ECT_REG_IRQMASK, sizeof(w), &w, EC_TIMEOUTRET3); /* set IRQ mask */
ecx_BWR(context->port, 0x0000, ECT_REG_RXERR, 8, &zbuf, EC_TIMEOUTRET3); /* reset CRC counters */
ecx_BWR(context->port, 0x0000, ECT_REG_FMMU0, 16 * 3, &zbuf, EC_TIMEOUTRET3); /* reset FMMU's */
ecx_BWR(context->port, 0x0000, ECT_REG_SM0, 8 * 4, &zbuf, EC_TIMEOUTRET3); /* reset SyncM */
ecx_BWR(context->port, 0x0000, ECT_REG_DCSYSTIME, 4, &zbuf, EC_TIMEOUTRET3); /* reset system time+ofs */
w = htoes(0x1000);
ecx_BWR(context->port, 0x0000, ECT_REG_DCSPEEDCNT, sizeof(w), &w, EC_TIMEOUTRET3); /* DC speedstart */
w = htoes(0x0c00);
ecx_BWR(context->port, 0x0000, ECT_REG_DCTIMEFILT, sizeof(w), &w, EC_TIMEOUTRET3); /* DC filt expr */
b = 0x00;
ecx_BWR(context->port, 0x0000, ECT_REG_DLALIAS, sizeof(b), &b, EC_TIMEOUTRET3); /* Ignore Alias register */
b = EC_STATE_INIT | EC_STATE_ACK;
ecx_BWR(context->port, 0x0000, ECT_REG_ALCTL, sizeof(b), &b, EC_TIMEOUTRET3); /* Reset all slaves to Init */
b = 2;
ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG, sizeof(b), &b , EC_TIMEOUTRET3); /* force Eeprom from PDI */
b = 0;
ecx_BWR(context->port, 0x0000, ECT_REG_EEPCFG, sizeof(b), &b , EC_TIMEOUTRET3); /* set Eeprom to master */
for (slave = 1; slave <= *(context->slavecount); slave++)
{
ADPh = (uint16)(1 - slave);
context->slavelist[slave].Itype =
etohs(ecx_APRDw(context->port, ADPh, ECT_REG_PDICTL, EC_TIMEOUTRET3)); /* read interface type of slave */
/* a node offset is used to improve readibility of network frames */
/* this has no impact on the number of addressable slaves (auto wrap around) */
ecx_APWRw(context->port, ADPh, ECT_REG_STADR, htoes(slave + EC_NODEOFFSET) , EC_TIMEOUTRET3); /* set node address of slave */
if (slave == 1)
{
b = 1; /* kill non ecat frames for first slave */
}
else
{
b = 0; /* pass all frames for following slaves */
}
ecx_APWRw(context->port, ADPh, ECT_REG_DLCTL, htoes(b), EC_TIMEOUTRET3); /* set non ecat frame behaviour */
configadr = etohs(ecx_APRDw(context->port, ADPh, ECT_REG_STADR, EC_TIMEOUTRET3));
context->slavelist[slave].configadr = configadr;
ecx_FPRD(context->port, configadr, ECT_REG_ALIAS, sizeof(aliasadr), &aliasadr, EC_TIMEOUTRET3);
context->slavelist[slave].aliasadr = etohs(aliasadr);
ecx_FPRD(context->port, configadr, ECT_REG_EEPSTAT, sizeof(estat), &estat, EC_TIMEOUTRET3);
estat = etohs(estat);
if (estat & EC_ESTAT_R64) /* check if slave can read 8 byte chunks */
{
context->slavelist[slave].eep_8byte = 1;
}
ecx_readeeprom1(context, slave, ECT_SII_MANUF); /* Manuf */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
context->slavelist[slave].eep_man =
etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* Manuf */
ecx_readeeprom1(context, slave, ECT_SII_ID); /* ID */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
context->slavelist[slave].eep_id =
etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* ID */
ecx_readeeprom1(context, slave, ECT_SII_REV); /* revision */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
context->slavelist[slave].eep_rev =
etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* revision */
ecx_readeeprom1(context, slave, ECT_SII_RXMBXADR); /* write mailbox address + mailboxsize */
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
eedat = etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* write mailbox address and mailboxsize */
context->slavelist[slave].mbx_wo = (uint16)LO_WORD(eedat);
context->slavelist[slave].mbx_l = (uint16)HI_WORD(eedat);
if (context->slavelist[slave].mbx_l > 0)
{
ecx_readeeprom1(context, slave, ECT_SII_TXMBXADR); /* read mailbox offset */
}
}
for (slave = 1; slave <= *(context->slavecount); slave++)
{
if (context->slavelist[slave].mbx_l > 0)
{
eedat = etohl(ecx_readeeprom2(context, slave, EC_TIMEOUTEEP)); /* read mailbox offset */
context->slavelist[slave].mbx_ro = (uint16)LO_WORD(eedat); /* read mailbox offset */
context->slavelist[slave].mbx_rl = (uint16)HI_WORD(eedat); /*read mailbox length */
if (context->slavelist[slave].mbx_rl == 0)
{
context->slavelist[slave].mbx_rl = context->slavelist[slave].mbx_l;
}
}
configadr = context->slavelist[slave].configadr;
if ((etohs(ecx_FPRDw(context->port, configadr, ECT_REG_ESCSUP, EC_TIMEOUTRET3)) & 0x04) > 0) /* Support DC? */
{
context->slavelist[slave].hasdc = TRUE;
}
else
{
context->slavelist[slave].hasdc = FALSE;
}
topology = etohs(ecx_FPRDw(context->port, configadr, ECT_REG_DLSTAT, EC_TIMEOUTRET3)); /* extract topology from DL status */
h = 0;
b = 0;
if ((topology & 0x0300) == 0x0200) /* port0 open and communication established */
{
h++;
b |= 0x01;
}
if ((topology & 0x0c00) == 0x0800) /* port1 open and communication established */
{
h++;
b |= 0x02;
}
if ((topology & 0x3000) == 0x2000) /* port2 open and communication established */
{
h++;
b |= 0x04;
}
if ((topology & 0xc000) == 0x8000) /* port3 open and communication established */
{
h++;
b |= 0x08;
}
/* ptype = Physical type*/
context->slavelist[slave].ptype =
LO_BYTE(etohs(ecx_FPRDw(context->port, configadr, ECT_REG_PORTDES, EC_TIMEOUTRET3)));
context->slavelist[slave].topology = h;
context->slavelist[slave].activeports = b;
/* 0=no links, not possible */
/* 1=1 link , end of line */
/* 2=2 links , one before and one after */
/* 3=3 links , split point */
/* 4=4 links , cross point */
/* search for parent */
context->slavelist[slave].parent = 0; /* parent is master */
if (slave > 1)
{
topoc = 0;
slavec = slave - 1;
do
{
topology = context->slavelist[slavec].topology;
if (topology == 1)
{
topoc--; /* endpoint found */
}
if (topology == 3)
{
topoc++; /* split found */
}
if (topology == 4)
{
topoc += 2; /* cross found */
}
if (((topoc >= 0) && (topology > 1)) ||
(slavec == 1)) /* parent found */
{
context->slavelist[slave].parent = slavec;
slavec = 1;
}
slavec--;
}
while (slavec > 0);
}
w = ecx_statecheck(context, slave, EC_STATE_INIT, EC_TIMEOUTSTATE); //* check state change Init */
/* set default mailbox configuration if slave has mailbox */
if (context->slavelist[slave].mbx_l>0)
{
context->slavelist[slave].SMtype[0] = 1;
context->slavelist[slave].SMtype[1] = 2;
context->slavelist[slave].SMtype[2] = 3;
context->slavelist[slave].SMtype[3] = 4;
context->slavelist[slave].SM[0].StartAddr = htoes(context->slavelist[slave].mbx_wo);
context->slavelist[slave].SM[0].SMlength = htoes(context->slavelist[slave].mbx_l);
context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
context->slavelist[slave].SM[1].StartAddr = htoes(context->slavelist[slave].mbx_ro);
context->slavelist[slave].SM[1].SMlength = htoes(context->slavelist[slave].mbx_rl);
context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
context->slavelist[slave].mbx_proto =
ecx_readeeprom(context, slave, ECT_SII_MBXPROTO, EC_TIMEOUTEEP);
}
cindex = 0;
#ifdef EC_VER1
/* use configuration table ? */
if (usetable)
{
cindex = ec_findconfig( context->slavelist[slave].eep_man, context->slavelist[slave].eep_id );
context->slavelist[slave].configindex= cindex;
}
/* slave found in configuration table ? */
if (cindex)
{
context->slavelist[slave].Dtype = ec_configlist[cindex].Dtype;
strcpy(context->slavelist[slave].name ,ec_configlist[cindex].name);
context->slavelist[slave].Ibits = ec_configlist[cindex].Ibits;
context->slavelist[slave].Obits = ec_configlist[cindex].Obits;
if (context->slavelist[slave].Obits)
{
context->slavelist[slave].FMMU0func = 1;
}
if (context->slavelist[slave].Ibits)
{
context->slavelist[slave].FMMU1func = 2;
}
context->slavelist[slave].FMMU[0].FMMUactive = ec_configlist[cindex].FM0ac;
context->slavelist[slave].FMMU[1].FMMUactive = ec_configlist[cindex].FM1ac;
context->slavelist[slave].SM[2].StartAddr = htoes(ec_configlist[cindex].SM2a);
context->slavelist[slave].SM[2].SMflags = htoel(ec_configlist[cindex].SM2f);
/* simple (no mailbox) output slave found ? */
if (context->slavelist[slave].Obits && !context->slavelist[slave].SM[2].StartAddr)
{
context->slavelist[slave].SM[0].StartAddr = htoes(0x0f00);
context->slavelist[slave].SM[0].SMlength = htoes((context->slavelist[slave].Obits + 7) / 8);
context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTDOSM0);
context->slavelist[slave].FMMU[0].FMMUactive = 1;
context->slavelist[slave].FMMU[0].FMMUtype = 2;
context->slavelist[slave].SMtype[0] = 3;
}
/* complex output slave */
else
{
context->slavelist[slave].SM[2].SMlength = htoes((context->slavelist[slave].Obits + 7) / 8);
context->slavelist[slave].SMtype[2] = 3;
}
context->slavelist[slave].SM[3].StartAddr = htoes(ec_configlist[cindex].SM3a);
context->slavelist[slave].SM[3].SMflags = htoel(ec_configlist[cindex].SM3f);
/* simple (no mailbox) input slave found ? */
if (context->slavelist[slave].Ibits && !context->slavelist[slave].SM[3].StartAddr)
{
context->slavelist[slave].SM[1].StartAddr = htoes(0x1000);
context->slavelist[slave].SM[1].SMlength = htoes((context->slavelist[slave].Ibits + 7) / 8);
context->slavelist[slave].SM[1].SMflags = htoel(0x00000000);
context->slavelist[slave].FMMU[1].FMMUactive = 1;
context->slavelist[slave].FMMU[1].FMMUtype = 1;
context->slavelist[slave].SMtype[1] = 4;
}
/* complex input slave */
else
{
context->slavelist[slave].SM[3].SMlength = htoes((context->slavelist[slave].Ibits + 7) / 8);
context->slavelist[slave].SMtype[3] = 4;
}
}
/* slave not in configuration table, find out via SII */
else
#endif
{
ssigen = ecx_siifind(context, slave, ECT_SII_GENERAL);
/* SII general section */
if (ssigen)
{
context->slavelist[slave].CoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x07);
context->slavelist[slave].FoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x08);
context->slavelist[slave].EoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x09);
context->slavelist[slave].SoEdetails = ecx_siigetbyte(context, slave, ssigen + 0x0a);
if((ecx_siigetbyte(context, slave, ssigen + 0x0d) & 0x02) > 0)
{
context->slavelist[slave].blockLRW = 1;
context->slavelist[0].blockLRW++;
}
context->slavelist[slave].Ebuscurrent = ecx_siigetbyte(context, slave, ssigen + 0x0e);
context->slavelist[slave].Ebuscurrent += ecx_siigetbyte(context, slave, ssigen + 0x0f) << 8;
context->slavelist[0].Ebuscurrent += context->slavelist[slave].Ebuscurrent;
}
/* SII strings section */
if (ecx_siifind(context, slave, ECT_SII_STRING) > 0)
{
ecx_siistring(context, context->slavelist[slave].name, slave, 1);
}
/* no name for slave found, use constructed name */
else
{
sprintf(context->slavelist[slave].name, "? M:%8.8x I:%8.8x",
(unsigned int)context->slavelist[slave].eep_man,
(unsigned int)context->slavelist[slave].eep_id);
}
/* SII SM section */
nSM = ecx_siiSM(context, slave, context->eepSM);
if (nSM>0)
{
context->slavelist[slave].SM[0].StartAddr = htoes(context->eepSM->PhStart);
context->slavelist[slave].SM[0].SMlength = htoes(context->eepSM->Plength);
context->slavelist[slave].SM[0].SMflags =
htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16));
SMc = 1;
while ((SMc < EC_MAXSM) && ecx_siiSMnext(context, slave, context->eepSM, SMc))
{
context->slavelist[slave].SM[SMc].StartAddr = htoes(context->eepSM->PhStart);
context->slavelist[slave].SM[SMc].SMlength = htoes(context->eepSM->Plength);
context->slavelist[slave].SM[SMc].SMflags =
htoel((context->eepSM->Creg) + (context->eepSM->Activate << 16));
SMc++;
}
}
/* SII FMMU section */
if (ecx_siiFMMU(context, slave, context->eepFMMU))
{
if (context->eepFMMU->FMMU0 !=0xff)
{
context->slavelist[slave].FMMU0func = context->eepFMMU->FMMU0;
}
if (context->eepFMMU->FMMU1 !=0xff)
{
context->slavelist[slave].FMMU1func = context->eepFMMU->FMMU1;
}
if (context->eepFMMU->FMMU2 !=0xff)
{
context->slavelist[slave].FMMU2func = context->eepFMMU->FMMU2;
}
if (context->eepFMMU->FMMU3 !=0xff)
{
context->slavelist[slave].FMMU3func = context->eepFMMU->FMMU3;
}
}
}
if (context->slavelist[slave].mbx_l > 0)
{
if (context->slavelist[slave].SM[0].StartAddr == 0x0000) /* should never happen */
{
EC_PRINT("Slave %d has no proper mailbox in configuration, try default.\n", slave);
context->slavelist[slave].SM[0].StartAddr = htoes(0x1000);
context->slavelist[slave].SM[0].SMlength = htoes(0x0080);
context->slavelist[slave].SM[0].SMflags = htoel(EC_DEFAULTMBXSM0);
context->slavelist[slave].SMtype[0] = 1;
}
if (context->slavelist[slave].SM[1].StartAddr == 0x0000) /* should never happen */
{
EC_PRINT("Slave %d has no proper mailbox out configuration, try default.\n", slave);
context->slavelist[slave].SM[1].StartAddr = htoes(0x1080);
context->slavelist[slave].SM[1].SMlength = htoes(0x0080);
context->slavelist[slave].SM[1].SMflags = htoel(EC_DEFAULTMBXSM1);
context->slavelist[slave].SMtype[1] = 2;
}
/* program SM0 mailbox in and SM1 mailbox out for slave */
/* writing both SM in one datagram will solve timing issue in old NETX */
ecx_FPWR(context->port, configadr, ECT_REG_SM0, sizeof(ec_smt) * 2,
&(context->slavelist[slave].SM[0]), EC_TIMEOUTRET3);
}
/* request pre_op for slave */
ecx_FPWRw(context->port, configadr, ECT_REG_ALCTL, htoes(EC_STATE_PRE_OP | EC_STATE_ACK) , EC_TIMEOUTRET3); /* set preop status */
}
}
return wkc;
}
/**
* Locate DC slaves, measure propagation delays.
*
* @param[in] context = context struct
* @return boolean if slaves are found with DC
*/
boolean ecx_configdc(ecx_contextt *context)
{
uint16 i, slaveh, parent, child;
uint16 parenthold = 0;
uint16 prevDCslave = 0;
int32 ht, dt1, dt2, dt3;
int64 hrt;
uint8 entryport;
int8 nlist;
int8 plist[4];
int32 tlist[4];
ec_timet mastertime;
uint64 mastertime64;
context->slavelist[0].hasdc = FALSE;
context->grouplist[0].hasdc = FALSE;
ht = 0;
ecx_BWR(context->port, 0, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET); /* latch DCrecvTimeA of all slaves */
mastertime = osal_current_time();
mastertime.sec -= 946684800UL; /* EtherCAT uses 2000-01-01 as epoch start instead of 1970-01-01 */
mastertime64 = (((uint64)mastertime.sec * 1000000) + (uint64)mastertime.usec) * 1000;
for (i = 1; i <= *(context->slavecount); i++)
{
context->slavelist[i].consumedports = context->slavelist[i].activeports;
if (context->slavelist[i].hasdc)
{
if (!context->slavelist[0].hasdc)
{
context->slavelist[0].hasdc = TRUE;
context->slavelist[0].DCnext = i;
context->slavelist[i].DCprevious = 0;
context->grouplist[0].hasdc = TRUE;
context->grouplist[0].DCnext = i;
}
else
{
context->slavelist[prevDCslave].DCnext = i;
context->slavelist[i].DCprevious = prevDCslave;
}
/* this branch has DC slave so remove parenthold */
parenthold = 0;
prevDCslave = i;
slaveh = context->slavelist[i].configadr;
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET);
context->slavelist[i].DCrtA = etohl(ht);
/* 64bit latched DCrecvTimeA of each specific slave */
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSOF, sizeof(hrt), &hrt, EC_TIMEOUTRET);
/* use it as offset in order to set local time around 0 + mastertime */
hrt = htoell(-etohll(hrt) + mastertime64);
/* save it in the offset register */
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSOFFSET, sizeof(hrt), &hrt, EC_TIMEOUTRET);
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME1, sizeof(ht), &ht, EC_TIMEOUTRET);
context->slavelist[i].DCrtB = etohl(ht);
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME2, sizeof(ht), &ht, EC_TIMEOUTRET);
context->slavelist[i].DCrtC = etohl(ht);
(void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME3, sizeof(ht), &ht, EC_TIMEOUTRET);
context->slavelist[i].DCrtD = etohl(ht);
/* make list of active ports and their time stamps */
nlist = 0;
if (context->slavelist[i].activeports & PORTM0)
{
plist[nlist] = 0;
tlist[nlist] = context->slavelist[i].DCrtA;
nlist++;
}
if (context->slavelist[i].activeports & PORTM3)
{
plist[nlist] = 3;
tlist[nlist] = context->slavelist[i].DCrtD;
nlist++;
}
if (context->slavelist[i].activeports & PORTM1)
{
plist[nlist] = 1;
tlist[nlist] = context->slavelist[i].DCrtB;
nlist++;
}
if (context->slavelist[i].activeports & PORTM2)
{
plist[nlist] = 2;
tlist[nlist] = context->slavelist[i].DCrtC;
nlist++;
}
/* entryport is port with the lowest timestamp */
entryport = 0;
if((nlist > 1) && (tlist[1] < tlist[entryport]))
{
entryport = 1;
}
if((nlist > 2) && (tlist[2] < tlist[entryport]))
{
entryport = 2;
}
if((nlist > 3) && (tlist[3] < tlist[entryport]))
{
entryport = 3;
}
entryport = plist[entryport];
context->slavelist[i].entryport = entryport;
/* consume entryport from activeports */
context->slavelist[i].consumedports &= (uint8)~(1 << entryport);
/* finding DC parent of current */
parent = i;
do
{
child = parent;
parent = context->slavelist[parent].parent;
}
while (!((parent == 0) || (context->slavelist[parent].hasdc)));
/* only calculate propagation delay if slave is not the first */
if (parent > 0)
{
/* find port on parent this slave is connected to */
context->slavelist[i].parentport = ecx_parentport(context, parent);
if (context->slavelist[parent].topology == 1)
{
context->slavelist[i].parentport = context->slavelist[parent].entryport;
}
dt1 = 0;
dt2 = 0;
/* delta time of (parentport - 1) - parentport */
/* note: order of ports is 0 - 3 - 1 -2 */
/* non active ports are skipped */
dt3 = ecx_porttime(context, parent, context->slavelist[i].parentport) -
ecx_porttime(context, parent,
ecx_prevport(context, parent, context->slavelist[i].parentport));
/* current slave has children */
/* those children's delays need to be subtracted */
if (context->slavelist[i].topology > 1)
{
dt1 = ecx_porttime(context, i,
ecx_prevport(context, i, context->slavelist[i].entryport)) -
ecx_porttime(context, i, context->slavelist[i].entryport);
}
/* we are only interested in positive difference */
if (dt1 > dt3) dt1 = -dt1;
/* current slave is not the first child of parent */
/* previous child's delays need to be added */
if ((child - parent) > 1)
{
dt2 = ecx_porttime(context, parent,
ecx_prevport(context, parent, context->slavelist[i].parentport)) -
ecx_porttime(context, parent, context->slavelist[parent].entryport);
}
if (dt2 < 0) dt2 = -dt2;
/* calculate current slave delay from delta times */
/* assumption : forward delay equals return delay */
context->slavelist[i].pdelay = ((dt3 - dt1) / 2) + dt2 +
context->slavelist[parent].pdelay;
ht = htoel(context->slavelist[i].pdelay);
/* write propagation delay*/
(void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSDELAY, sizeof(ht), &ht, EC_TIMEOUTRET);
}
}
else
{
context->slavelist[i].DCrtA = 0;
context->slavelist[i].DCrtB = 0;
context->slavelist[i].DCrtC = 0;
context->slavelist[i].DCrtD = 0;
parent = context->slavelist[i].parent;
/* if non DC slave found on first position on branch hold root parent */
if ( (parent > 0) && (context->slavelist[parent].topology > 2))
parenthold = parent;
/* if branch has no DC slaves consume port on root parent */
if ( parenthold && (context->slavelist[i].topology == 1))
{
ecx_parentport(context, parenthold);
parenthold = 0;
}
}
}
return context->slavelist[0].hasdc;
}
