AB&T / SOEM

SOEM EtherCAT Master library for STM Nucleo F767ZI

Dependents:   EasyCAT_LAB_simple EasyCAT_LAB_very_simple EasyCAT_LAB

  • It has been developed for the EasyCAT LAB , a complete educational and experimental EtherCAT® system, composed of one master and two slaves .

Warning

/media/uploads/EasyCAT/easycat_lab.jpg

SOEM/ethercatdc.c@0:543d6784d4cc, 2019-06-11 (annotated)

Committer:
EasyCAT
Date:
Tue Jun 11 10:29:09 2019 +0000
Revision:
0:543d6784d4cc
SOEM EtherCAT Master Library for STM Nucleo F767ZI

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EasyCAT 0:543d6784d4cc 1 /*
EasyCAT 0:543d6784d4cc 2 * Licensed under the GNU General Public License version 2 with exceptions. See
EasyCAT 0:543d6784d4cc 3 * LICENSE file in the project root for full license information
EasyCAT 0:543d6784d4cc 4 */
EasyCAT 0:543d6784d4cc 5
EasyCAT 0:543d6784d4cc 6 /** \file
EasyCAT 0:543d6784d4cc 7 * \brief
EasyCAT 0:543d6784d4cc 8 * Distributed Clock EtherCAT functions.
EasyCAT 0:543d6784d4cc 9 *
EasyCAT 0:543d6784d4cc 10 */
EasyCAT 0:543d6784d4cc 11 #include "oshw.h"
EasyCAT 0:543d6784d4cc 12 #include "osal.h"
EasyCAT 0:543d6784d4cc 13 #include "ethercattype.h"
EasyCAT 0:543d6784d4cc 14 #include "ethercatbase.h"
EasyCAT 0:543d6784d4cc 15 #include "ethercatmain.h"
EasyCAT 0:543d6784d4cc 16 #include "ethercatdc.h"
EasyCAT 0:543d6784d4cc 17
EasyCAT 0:543d6784d4cc 18 #define PORTM0 0x01
EasyCAT 0:543d6784d4cc 19 #define PORTM1 0x02
EasyCAT 0:543d6784d4cc 20 #define PORTM2 0x04
EasyCAT 0:543d6784d4cc 21 #define PORTM3 0x08
EasyCAT 0:543d6784d4cc 22
EasyCAT 0:543d6784d4cc 23 /** 1st sync pulse delay in ns here 100ms */
EasyCAT 0:543d6784d4cc 24 #define SyncDelay ((int32)100000000)
EasyCAT 0:543d6784d4cc 25
EasyCAT 0:543d6784d4cc 26 /**
EasyCAT 0:543d6784d4cc 27 * Set DC of slave to fire sync0 at CyclTime interval with CyclShift offset.
EasyCAT 0:543d6784d4cc 28 *
EasyCAT 0:543d6784d4cc 29 * @param[in] context = context struct
EasyCAT 0:543d6784d4cc 30 * @param [in] slave Slave number.
EasyCAT 0:543d6784d4cc 31 * @param [in] act TRUE = active, FALSE = deactivated
EasyCAT 0:543d6784d4cc 32 * @param [in] CyclTime Cycltime in ns.
EasyCAT 0:543d6784d4cc 33 * @param [in] CyclShift CyclShift in ns.
EasyCAT 0:543d6784d4cc 34 */
EasyCAT 0:543d6784d4cc 35 void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
EasyCAT 0:543d6784d4cc 36 {
EasyCAT 0:543d6784d4cc 37 uint8 h, RA;
EasyCAT 0:543d6784d4cc 38 uint16 slaveh;
EasyCAT 0:543d6784d4cc 39 int64 t, t1;
EasyCAT 0:543d6784d4cc 40 int32 tc;
EasyCAT 0:543d6784d4cc 41
EasyCAT 0:543d6784d4cc 42 slaveh = context->slavelist[slave].configadr;
EasyCAT 0:543d6784d4cc 43 RA = 0;
EasyCAT 0:543d6784d4cc 44
EasyCAT 0:543d6784d4cc 45 /* stop cyclic operation, ready for next trigger */
EasyCAT 0:543d6784d4cc 46 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
EasyCAT 0:543d6784d4cc 47 if (act)
EasyCAT 0:543d6784d4cc 48 {
EasyCAT 0:543d6784d4cc 49 RA = 1 + 2; /* act cyclic operation and sync0, sync1 deactivated */
EasyCAT 0:543d6784d4cc 50 }
EasyCAT 0:543d6784d4cc 51 h = 0;
EasyCAT 0:543d6784d4cc 52 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
EasyCAT 0:543d6784d4cc 53 t1 = 0;
EasyCAT 0:543d6784d4cc 54 (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
EasyCAT 0:543d6784d4cc 55 t1 = etohll(t1);
EasyCAT 0:543d6784d4cc 56
EasyCAT 0:543d6784d4cc 57 /* Calculate first trigger time, always a whole multiple of CyclTime rounded up
EasyCAT 0:543d6784d4cc 58 plus the shifttime (can be negative)
EasyCAT 0:543d6784d4cc 59 This insures best synchronization between slaves, slaves with the same CyclTime
EasyCAT 0:543d6784d4cc 60 will sync at the same moment (you can use CyclShift to shift the sync) */
EasyCAT 0:543d6784d4cc 61 if (CyclTime > 0)
EasyCAT 0:543d6784d4cc 62 {
EasyCAT 0:543d6784d4cc 63 t = ((t1 + SyncDelay) / CyclTime) * CyclTime + CyclTime + CyclShift;
EasyCAT 0:543d6784d4cc 64 }
EasyCAT 0:543d6784d4cc 65 else
EasyCAT 0:543d6784d4cc 66 {
EasyCAT 0:543d6784d4cc 67 t = t1 + SyncDelay + CyclShift;
EasyCAT 0:543d6784d4cc 68 /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
EasyCAT 0:543d6784d4cc 69 }
EasyCAT 0:543d6784d4cc 70 t = htoell(t);
EasyCAT 0:543d6784d4cc 71 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
EasyCAT 0:543d6784d4cc 72 tc = htoel(CyclTime);
EasyCAT 0:543d6784d4cc 73 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
EasyCAT 0:543d6784d4cc 74 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
EasyCAT 0:543d6784d4cc 75
EasyCAT 0:543d6784d4cc 76 // update ec_slave state
EasyCAT 0:543d6784d4cc 77 context->slavelist[slave].DCactive = (uint8)act;
EasyCAT 0:543d6784d4cc 78 context->slavelist[slave].DCshift = CyclShift;
EasyCAT 0:543d6784d4cc 79 context->slavelist[slave].DCcycle = CyclTime;
EasyCAT 0:543d6784d4cc 80 }
EasyCAT 0:543d6784d4cc 81
EasyCAT 0:543d6784d4cc 82 /**
EasyCAT 0:543d6784d4cc 83 * Set DC of slave to fire sync0 and sync1 at CyclTime interval with CyclShift offset.
EasyCAT 0:543d6784d4cc 84 *
EasyCAT 0:543d6784d4cc 85 * @param[in] context = context struct
EasyCAT 0:543d6784d4cc 86 * @param [in] slave Slave number.
EasyCAT 0:543d6784d4cc 87 * @param [in] act TRUE = active, FALSE = deactivated
EasyCAT 0:543d6784d4cc 88 * @param [in] CyclTime0 Cycltime SYNC0 in ns.
EasyCAT 0:543d6784d4cc 89 * @param [in] CyclTime1 Cycltime SYNC1 in ns. This time is a delta time in relation to
EasyCAT 0:543d6784d4cc 90 the SYNC0 fire. If CylcTime1 = 0 then SYNC1 fires a the same time
EasyCAT 0:543d6784d4cc 91 as SYNC0.
EasyCAT 0:543d6784d4cc 92 * @param [in] CyclShift CyclShift in ns.
EasyCAT 0:543d6784d4cc 93 */
EasyCAT 0:543d6784d4cc 94 void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift)
EasyCAT 0:543d6784d4cc 95 {
EasyCAT 0:543d6784d4cc 96 uint8 h, RA;
EasyCAT 0:543d6784d4cc 97 uint16 slaveh;
EasyCAT 0:543d6784d4cc 98 int64 t, t1;
EasyCAT 0:543d6784d4cc 99 int32 tc;
EasyCAT 0:543d6784d4cc 100 uint32 TrueCyclTime;
EasyCAT 0:543d6784d4cc 101
EasyCAT 0:543d6784d4cc 102 /* Sync1 can be used as a multiple of Sync0, use true cycle time */
EasyCAT 0:543d6784d4cc 103 TrueCyclTime = ((CyclTime1 / CyclTime0) + 1) * CyclTime0;
EasyCAT 0:543d6784d4cc 104
EasyCAT 0:543d6784d4cc 105 slaveh = context->slavelist[slave].configadr;
EasyCAT 0:543d6784d4cc 106 RA = 0;
EasyCAT 0:543d6784d4cc 107
EasyCAT 0:543d6784d4cc 108 /* stop cyclic operation, ready for next trigger */
EasyCAT 0:543d6784d4cc 109 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET);
EasyCAT 0:543d6784d4cc 110 if (act)
EasyCAT 0:543d6784d4cc 111 {
EasyCAT 0:543d6784d4cc 112 RA = 1 + 2 + 4; /* act cyclic operation and sync0 + sync1 */
EasyCAT 0:543d6784d4cc 113 }
EasyCAT 0:543d6784d4cc 114 h = 0;
EasyCAT 0:543d6784d4cc 115 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */
EasyCAT 0:543d6784d4cc 116 t1 = 0;
EasyCAT 0:543d6784d4cc 117 (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */
EasyCAT 0:543d6784d4cc 118 t1 = etohll(t1);
EasyCAT 0:543d6784d4cc 119
EasyCAT 0:543d6784d4cc 120 /* Calculate first trigger time, always a whole multiple of TrueCyclTime rounded up
EasyCAT 0:543d6784d4cc 121 plus the shifttime (can be negative)
EasyCAT 0:543d6784d4cc 122 This insures best synchronization between slaves, slaves with the same CyclTime
EasyCAT 0:543d6784d4cc 123 will sync at the same moment (you can use CyclShift to shift the sync) */
EasyCAT 0:543d6784d4cc 124 if (CyclTime0 > 0)
EasyCAT 0:543d6784d4cc 125 {
EasyCAT 0:543d6784d4cc 126 t = ((t1 + SyncDelay) / TrueCyclTime) * TrueCyclTime + TrueCyclTime + CyclShift;
EasyCAT 0:543d6784d4cc 127 }
EasyCAT 0:543d6784d4cc 128 else
EasyCAT 0:543d6784d4cc 129 {
EasyCAT 0:543d6784d4cc 130 t = t1 + SyncDelay + CyclShift;
EasyCAT 0:543d6784d4cc 131 /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */
EasyCAT 0:543d6784d4cc 132 }
EasyCAT 0:543d6784d4cc 133 t = htoell(t);
EasyCAT 0:543d6784d4cc 134 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */
EasyCAT 0:543d6784d4cc 135 tc = htoel(CyclTime0);
EasyCAT 0:543d6784d4cc 136 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */
EasyCAT 0:543d6784d4cc 137 tc = htoel(CyclTime1);
EasyCAT 0:543d6784d4cc 138 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE1, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC1 cycle time */
EasyCAT 0:543d6784d4cc 139 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */
EasyCAT 0:543d6784d4cc 140
EasyCAT 0:543d6784d4cc 141 // update ec_slave state
EasyCAT 0:543d6784d4cc 142 context->slavelist[slave].DCactive = (uint8)act;
EasyCAT 0:543d6784d4cc 143 context->slavelist[slave].DCshift = CyclShift;
EasyCAT 0:543d6784d4cc 144 context->slavelist[slave].DCcycle = CyclTime0;
EasyCAT 0:543d6784d4cc 145 }
EasyCAT 0:543d6784d4cc 146
EasyCAT 0:543d6784d4cc 147 /* latched port time of slave */
EasyCAT 0:543d6784d4cc 148 static int32 ecx_porttime(ecx_contextt *context, uint16 slave, uint8 port)
EasyCAT 0:543d6784d4cc 149 {
EasyCAT 0:543d6784d4cc 150 int32 ts;
EasyCAT 0:543d6784d4cc 151 switch (port)
EasyCAT 0:543d6784d4cc 152 {
EasyCAT 0:543d6784d4cc 153 case 0:
EasyCAT 0:543d6784d4cc 154 ts = context->slavelist[slave].DCrtA;
EasyCAT 0:543d6784d4cc 155 break;
EasyCAT 0:543d6784d4cc 156 case 1:
EasyCAT 0:543d6784d4cc 157 ts = context->slavelist[slave].DCrtB;
EasyCAT 0:543d6784d4cc 158 break;
EasyCAT 0:543d6784d4cc 159 case 2:
EasyCAT 0:543d6784d4cc 160 ts = context->slavelist[slave].DCrtC;
EasyCAT 0:543d6784d4cc 161 break;
EasyCAT 0:543d6784d4cc 162 case 3:
EasyCAT 0:543d6784d4cc 163 ts = context->slavelist[slave].DCrtD;
EasyCAT 0:543d6784d4cc 164 break;
EasyCAT 0:543d6784d4cc 165 default:
EasyCAT 0:543d6784d4cc 166 ts = 0;
EasyCAT 0:543d6784d4cc 167 break;
EasyCAT 0:543d6784d4cc 168 }
EasyCAT 0:543d6784d4cc 169 return ts;
EasyCAT 0:543d6784d4cc 170 }
EasyCAT 0:543d6784d4cc 171
EasyCAT 0:543d6784d4cc 172 /* calculate previous active port of a slave */
EasyCAT 0:543d6784d4cc 173 static uint8 ecx_prevport(ecx_contextt *context, uint16 slave, uint8 port)
EasyCAT 0:543d6784d4cc 174 {
EasyCAT 0:543d6784d4cc 175 uint8 pport = port;
EasyCAT 0:543d6784d4cc 176 uint8 aport = context->slavelist[slave].activeports;
EasyCAT 0:543d6784d4cc 177 switch(port)
EasyCAT 0:543d6784d4cc 178 {
EasyCAT 0:543d6784d4cc 179 case 0:
EasyCAT 0:543d6784d4cc 180 if(aport & PORTM2)
EasyCAT 0:543d6784d4cc 181 pport = 2;
EasyCAT 0:543d6784d4cc 182 else if (aport & PORTM1)
EasyCAT 0:543d6784d4cc 183 pport = 1;
EasyCAT 0:543d6784d4cc 184 else if (aport & PORTM3)
EasyCAT 0:543d6784d4cc 185 pport = 3;
EasyCAT 0:543d6784d4cc 186 break;
EasyCAT 0:543d6784d4cc 187 case 1:
EasyCAT 0:543d6784d4cc 188 if(aport & PORTM3)
EasyCAT 0:543d6784d4cc 189 pport = 3;
EasyCAT 0:543d6784d4cc 190 else if (aport & PORTM0)
EasyCAT 0:543d6784d4cc 191 pport = 0;
EasyCAT 0:543d6784d4cc 192 else if (aport & PORTM2)
EasyCAT 0:543d6784d4cc 193 pport = 2;
EasyCAT 0:543d6784d4cc 194 break;
EasyCAT 0:543d6784d4cc 195 case 2:
EasyCAT 0:543d6784d4cc 196 if(aport & PORTM1)
EasyCAT 0:543d6784d4cc 197 pport = 1;
EasyCAT 0:543d6784d4cc 198 else if (aport & PORTM3)
EasyCAT 0:543d6784d4cc 199 pport = 3;
EasyCAT 0:543d6784d4cc 200 else if (aport & PORTM0)
EasyCAT 0:543d6784d4cc 201 pport = 0;
EasyCAT 0:543d6784d4cc 202 break;
EasyCAT 0:543d6784d4cc 203 case 3:
EasyCAT 0:543d6784d4cc 204 if(aport & PORTM0)
EasyCAT 0:543d6784d4cc 205 pport = 0;
EasyCAT 0:543d6784d4cc 206 else if (aport & PORTM2)
EasyCAT 0:543d6784d4cc 207 pport = 2;
EasyCAT 0:543d6784d4cc 208 else if (aport & PORTM1)
EasyCAT 0:543d6784d4cc 209 pport = 1;
EasyCAT 0:543d6784d4cc 210 break;
EasyCAT 0:543d6784d4cc 211 }
EasyCAT 0:543d6784d4cc 212 return pport;
EasyCAT 0:543d6784d4cc 213 }
EasyCAT 0:543d6784d4cc 214
EasyCAT 0:543d6784d4cc 215 /* search unconsumed ports in parent, consume and return first open port */
EasyCAT 0:543d6784d4cc 216 static uint8 ecx_parentport(ecx_contextt *context, uint16 parent)
EasyCAT 0:543d6784d4cc 217 {
EasyCAT 0:543d6784d4cc 218 uint8 parentport = 0;
EasyCAT 0:543d6784d4cc 219 uint8 b;
EasyCAT 0:543d6784d4cc 220 /* search order is important, here 3 - 1 - 2 - 0 */
EasyCAT 0:543d6784d4cc 221 b = context->slavelist[parent].consumedports;
EasyCAT 0:543d6784d4cc 222 if (b & PORTM3)
EasyCAT 0:543d6784d4cc 223 {
EasyCAT 0:543d6784d4cc 224 parentport = 3;
EasyCAT 0:543d6784d4cc 225 b &= (uint8)~PORTM3;
EasyCAT 0:543d6784d4cc 226 }
EasyCAT 0:543d6784d4cc 227 else if (b & PORTM1)
EasyCAT 0:543d6784d4cc 228 {
EasyCAT 0:543d6784d4cc 229 parentport = 1;
EasyCAT 0:543d6784d4cc 230 b &= (uint8)~PORTM1;
EasyCAT 0:543d6784d4cc 231 }
EasyCAT 0:543d6784d4cc 232 else if (b & PORTM2)
EasyCAT 0:543d6784d4cc 233 {
EasyCAT 0:543d6784d4cc 234 parentport = 2;
EasyCAT 0:543d6784d4cc 235 b &= (uint8)~PORTM2;
EasyCAT 0:543d6784d4cc 236 }
EasyCAT 0:543d6784d4cc 237 else if (b & PORTM0)
EasyCAT 0:543d6784d4cc 238 {
EasyCAT 0:543d6784d4cc 239 parentport = 0;
EasyCAT 0:543d6784d4cc 240 b &= (uint8)~PORTM0;
EasyCAT 0:543d6784d4cc 241 }
EasyCAT 0:543d6784d4cc 242 context->slavelist[parent].consumedports = b;
EasyCAT 0:543d6784d4cc 243 return parentport;
EasyCAT 0:543d6784d4cc 244 }
EasyCAT 0:543d6784d4cc 245
EasyCAT 0:543d6784d4cc 246 /**
EasyCAT 0:543d6784d4cc 247 * Locate DC slaves, measure propagation delays.
EasyCAT 0:543d6784d4cc 248 *
EasyCAT 0:543d6784d4cc 249 * @param[in] context = context struct
EasyCAT 0:543d6784d4cc 250 * @return boolean if slaves are found with DC
EasyCAT 0:543d6784d4cc 251 */
EasyCAT 0:543d6784d4cc 252 boolean ecx_configdc(ecx_contextt *context)
EasyCAT 0:543d6784d4cc 253 {
EasyCAT 0:543d6784d4cc 254 uint16 i, slaveh, parent, child;
EasyCAT 0:543d6784d4cc 255 uint16 parenthold = 0;
EasyCAT 0:543d6784d4cc 256 uint16 prevDCslave = 0;
EasyCAT 0:543d6784d4cc 257 int32 ht, dt1, dt2, dt3;
EasyCAT 0:543d6784d4cc 258 int64 hrt;
EasyCAT 0:543d6784d4cc 259 uint8 entryport;
EasyCAT 0:543d6784d4cc 260 int8 nlist;
EasyCAT 0:543d6784d4cc 261 int8 plist[4];
EasyCAT 0:543d6784d4cc 262 int32 tlist[4];
EasyCAT 0:543d6784d4cc 263 ec_timet mastertime;
EasyCAT 0:543d6784d4cc 264 uint64 mastertime64;
EasyCAT 0:543d6784d4cc 265
EasyCAT 0:543d6784d4cc 266 context->slavelist[0].hasdc = FALSE;
EasyCAT 0:543d6784d4cc 267 context->grouplist[0].hasdc = FALSE;
EasyCAT 0:543d6784d4cc 268 ht = 0;
EasyCAT 0:543d6784d4cc 269
EasyCAT 0:543d6784d4cc 270 ecx_BWR(context->port, 0, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET); /* latch DCrecvTimeA of all slaves */
EasyCAT 0:543d6784d4cc 271 mastertime = osal_current_time();
EasyCAT 0:543d6784d4cc 272 mastertime.sec -= 946684800UL; /* EtherCAT uses 2000-01-01 as epoch start instead of 1970-01-01 */
EasyCAT 0:543d6784d4cc 273 mastertime64 = (((uint64)mastertime.sec * 1000000) + (uint64)mastertime.usec) * 1000;
EasyCAT 0:543d6784d4cc 274 for (i = 1; i <= *(context->slavecount); i++)
EasyCAT 0:543d6784d4cc 275 {
EasyCAT 0:543d6784d4cc 276 context->slavelist[i].consumedports = context->slavelist[i].activeports;
EasyCAT 0:543d6784d4cc 277 if (context->slavelist[i].hasdc)
EasyCAT 0:543d6784d4cc 278 {
EasyCAT 0:543d6784d4cc 279 if (!context->slavelist[0].hasdc)
EasyCAT 0:543d6784d4cc 280 {
EasyCAT 0:543d6784d4cc 281 context->slavelist[0].hasdc = TRUE;
EasyCAT 0:543d6784d4cc 282 context->slavelist[0].DCnext = i;
EasyCAT 0:543d6784d4cc 283 context->slavelist[i].DCprevious = 0;
EasyCAT 0:543d6784d4cc 284 context->grouplist[context->slavelist[i].group].hasdc = TRUE;
EasyCAT 0:543d6784d4cc 285 context->grouplist[context->slavelist[i].group].DCnext = i;
EasyCAT 0:543d6784d4cc 286 }
EasyCAT 0:543d6784d4cc 287 else
EasyCAT 0:543d6784d4cc 288 {
EasyCAT 0:543d6784d4cc 289 context->slavelist[prevDCslave].DCnext = i;
EasyCAT 0:543d6784d4cc 290 context->slavelist[i].DCprevious = prevDCslave;
EasyCAT 0:543d6784d4cc 291 }
EasyCAT 0:543d6784d4cc 292 /* this branch has DC slave so remove parenthold */
EasyCAT 0:543d6784d4cc 293 parenthold = 0;
EasyCAT 0:543d6784d4cc 294 prevDCslave = i;
EasyCAT 0:543d6784d4cc 295 slaveh = context->slavelist[i].configadr;
EasyCAT 0:543d6784d4cc 296 (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET);
EasyCAT 0:543d6784d4cc 297 context->slavelist[i].DCrtA = etohl(ht);
EasyCAT 0:543d6784d4cc 298 /* 64bit latched DCrecvTimeA of each specific slave */
EasyCAT 0:543d6784d4cc 299 (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCSOF, sizeof(hrt), &hrt, EC_TIMEOUTRET);
EasyCAT 0:543d6784d4cc 300 /* use it as offset in order to set local time around 0 + mastertime */
EasyCAT 0:543d6784d4cc 301 hrt = htoell(-etohll(hrt) + mastertime64);
EasyCAT 0:543d6784d4cc 302 /* save it in the offset register */
EasyCAT 0:543d6784d4cc 303 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSOFFSET, sizeof(hrt), &hrt, EC_TIMEOUTRET);
EasyCAT 0:543d6784d4cc 304 (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME1, sizeof(ht), &ht, EC_TIMEOUTRET);
EasyCAT 0:543d6784d4cc 305 context->slavelist[i].DCrtB = etohl(ht);
EasyCAT 0:543d6784d4cc 306 (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME2, sizeof(ht), &ht, EC_TIMEOUTRET);
EasyCAT 0:543d6784d4cc 307 context->slavelist[i].DCrtC = etohl(ht);
EasyCAT 0:543d6784d4cc 308 (void)ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME3, sizeof(ht), &ht, EC_TIMEOUTRET);
EasyCAT 0:543d6784d4cc 309 context->slavelist[i].DCrtD = etohl(ht);
EasyCAT 0:543d6784d4cc 310
EasyCAT 0:543d6784d4cc 311 /* make list of active ports and their time stamps */
EasyCAT 0:543d6784d4cc 312 nlist = 0;
EasyCAT 0:543d6784d4cc 313 if (context->slavelist[i].activeports & PORTM0)
EasyCAT 0:543d6784d4cc 314 {
EasyCAT 0:543d6784d4cc 315 plist[nlist] = 0;
EasyCAT 0:543d6784d4cc 316 tlist[nlist] = context->slavelist[i].DCrtA;
EasyCAT 0:543d6784d4cc 317 nlist++;
EasyCAT 0:543d6784d4cc 318 }
EasyCAT 0:543d6784d4cc 319 if (context->slavelist[i].activeports & PORTM3)
EasyCAT 0:543d6784d4cc 320 {
EasyCAT 0:543d6784d4cc 321 plist[nlist] = 3;
EasyCAT 0:543d6784d4cc 322 tlist[nlist] = context->slavelist[i].DCrtD;
EasyCAT 0:543d6784d4cc 323 nlist++;
EasyCAT 0:543d6784d4cc 324 }
EasyCAT 0:543d6784d4cc 325 if (context->slavelist[i].activeports & PORTM1)
EasyCAT 0:543d6784d4cc 326 {
EasyCAT 0:543d6784d4cc 327 plist[nlist] = 1;
EasyCAT 0:543d6784d4cc 328 tlist[nlist] = context->slavelist[i].DCrtB;
EasyCAT 0:543d6784d4cc 329 nlist++;
EasyCAT 0:543d6784d4cc 330 }
EasyCAT 0:543d6784d4cc 331 if (context->slavelist[i].activeports & PORTM2)
EasyCAT 0:543d6784d4cc 332 {
EasyCAT 0:543d6784d4cc 333 plist[nlist] = 2;
EasyCAT 0:543d6784d4cc 334 tlist[nlist] = context->slavelist[i].DCrtC;
EasyCAT 0:543d6784d4cc 335 nlist++;
EasyCAT 0:543d6784d4cc 336 }
EasyCAT 0:543d6784d4cc 337 /* entryport is port with the lowest timestamp */
EasyCAT 0:543d6784d4cc 338 entryport = 0;
EasyCAT 0:543d6784d4cc 339 if((nlist > 1) && (tlist[1] < tlist[entryport]))
EasyCAT 0:543d6784d4cc 340 {
EasyCAT 0:543d6784d4cc 341 entryport = 1;
EasyCAT 0:543d6784d4cc 342 }
EasyCAT 0:543d6784d4cc 343 if((nlist > 2) && (tlist[2] < tlist[entryport]))
EasyCAT 0:543d6784d4cc 344 {
EasyCAT 0:543d6784d4cc 345 entryport = 2;
EasyCAT 0:543d6784d4cc 346 }
EasyCAT 0:543d6784d4cc 347 if((nlist > 3) && (tlist[3] < tlist[entryport]))
EasyCAT 0:543d6784d4cc 348 {
EasyCAT 0:543d6784d4cc 349 entryport = 3;
EasyCAT 0:543d6784d4cc 350 }
EasyCAT 0:543d6784d4cc 351 entryport = plist[entryport];
EasyCAT 0:543d6784d4cc 352 context->slavelist[i].entryport = entryport;
EasyCAT 0:543d6784d4cc 353 /* consume entryport from activeports */
EasyCAT 0:543d6784d4cc 354 context->slavelist[i].consumedports &= (uint8)~(1 << entryport);
EasyCAT 0:543d6784d4cc 355
EasyCAT 0:543d6784d4cc 356 /* finding DC parent of current */
EasyCAT 0:543d6784d4cc 357 parent = i;
EasyCAT 0:543d6784d4cc 358 do
EasyCAT 0:543d6784d4cc 359 {
EasyCAT 0:543d6784d4cc 360 child = parent;
EasyCAT 0:543d6784d4cc 361 parent = context->slavelist[parent].parent;
EasyCAT 0:543d6784d4cc 362 }
EasyCAT 0:543d6784d4cc 363 while (!((parent == 0) || (context->slavelist[parent].hasdc)));
EasyCAT 0:543d6784d4cc 364 /* only calculate propagation delay if slave is not the first */
EasyCAT 0:543d6784d4cc 365 if (parent > 0)
EasyCAT 0:543d6784d4cc 366 {
EasyCAT 0:543d6784d4cc 367 /* find port on parent this slave is connected to */
EasyCAT 0:543d6784d4cc 368 context->slavelist[i].parentport = ecx_parentport(context, parent);
EasyCAT 0:543d6784d4cc 369 if (context->slavelist[parent].topology == 1)
EasyCAT 0:543d6784d4cc 370 {
EasyCAT 0:543d6784d4cc 371 context->slavelist[i].parentport = context->slavelist[parent].entryport;
EasyCAT 0:543d6784d4cc 372 }
EasyCAT 0:543d6784d4cc 373
EasyCAT 0:543d6784d4cc 374 dt1 = 0;
EasyCAT 0:543d6784d4cc 375 dt2 = 0;
EasyCAT 0:543d6784d4cc 376 /* delta time of (parentport - 1) - parentport */
EasyCAT 0:543d6784d4cc 377 /* note: order of ports is 0 - 3 - 1 -2 */
EasyCAT 0:543d6784d4cc 378 /* non active ports are skipped */
EasyCAT 0:543d6784d4cc 379 dt3 = ecx_porttime(context, parent, context->slavelist[i].parentport) -
EasyCAT 0:543d6784d4cc 380 ecx_porttime(context, parent,
EasyCAT 0:543d6784d4cc 381 ecx_prevport(context, parent, context->slavelist[i].parentport));
EasyCAT 0:543d6784d4cc 382 /* current slave has children */
EasyCAT 0:543d6784d4cc 383 /* those children's delays need to be subtracted */
EasyCAT 0:543d6784d4cc 384 if (context->slavelist[i].topology > 1)
EasyCAT 0:543d6784d4cc 385 {
EasyCAT 0:543d6784d4cc 386 dt1 = ecx_porttime(context, i,
EasyCAT 0:543d6784d4cc 387 ecx_prevport(context, i, context->slavelist[i].entryport)) -
EasyCAT 0:543d6784d4cc 388 ecx_porttime(context, i, context->slavelist[i].entryport);
EasyCAT 0:543d6784d4cc 389 }
EasyCAT 0:543d6784d4cc 390 /* we are only interested in positive difference */
EasyCAT 0:543d6784d4cc 391 if (dt1 > dt3) dt1 = -dt1;
EasyCAT 0:543d6784d4cc 392 /* current slave is not the first child of parent */
EasyCAT 0:543d6784d4cc 393 /* previous child's delays need to be added */
EasyCAT 0:543d6784d4cc 394 if ((child - parent) > 1)
EasyCAT 0:543d6784d4cc 395 {
EasyCAT 0:543d6784d4cc 396 dt2 = ecx_porttime(context, parent,
EasyCAT 0:543d6784d4cc 397 ecx_prevport(context, parent, context->slavelist[i].parentport)) -
EasyCAT 0:543d6784d4cc 398 ecx_porttime(context, parent, context->slavelist[parent].entryport);
EasyCAT 0:543d6784d4cc 399 }
EasyCAT 0:543d6784d4cc 400 if (dt2 < 0) dt2 = -dt2;
EasyCAT 0:543d6784d4cc 401
EasyCAT 0:543d6784d4cc 402 /* calculate current slave delay from delta times */
EasyCAT 0:543d6784d4cc 403 /* assumption : forward delay equals return delay */
EasyCAT 0:543d6784d4cc 404 context->slavelist[i].pdelay = ((dt3 - dt1) / 2) + dt2 +
EasyCAT 0:543d6784d4cc 405 context->slavelist[parent].pdelay;
EasyCAT 0:543d6784d4cc 406 ht = htoel(context->slavelist[i].pdelay);
EasyCAT 0:543d6784d4cc 407 /* write propagation delay*/
EasyCAT 0:543d6784d4cc 408 (void)ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSDELAY, sizeof(ht), &ht, EC_TIMEOUTRET);
EasyCAT 0:543d6784d4cc 409 }
EasyCAT 0:543d6784d4cc 410 }
EasyCAT 0:543d6784d4cc 411 else
EasyCAT 0:543d6784d4cc 412 {
EasyCAT 0:543d6784d4cc 413 context->slavelist[i].DCrtA = 0;
EasyCAT 0:543d6784d4cc 414 context->slavelist[i].DCrtB = 0;
EasyCAT 0:543d6784d4cc 415 context->slavelist[i].DCrtC = 0;
EasyCAT 0:543d6784d4cc 416 context->slavelist[i].DCrtD = 0;
EasyCAT 0:543d6784d4cc 417 parent = context->slavelist[i].parent;
EasyCAT 0:543d6784d4cc 418 /* if non DC slave found on first position on branch hold root parent */
EasyCAT 0:543d6784d4cc 419 if ( (parent > 0) && (context->slavelist[parent].topology > 2))
EasyCAT 0:543d6784d4cc 420 parenthold = parent;
EasyCAT 0:543d6784d4cc 421 /* if branch has no DC slaves consume port on root parent */
EasyCAT 0:543d6784d4cc 422 if ( parenthold && (context->slavelist[i].topology == 1))
EasyCAT 0:543d6784d4cc 423 {
EasyCAT 0:543d6784d4cc 424 ecx_parentport(context, parenthold);
EasyCAT 0:543d6784d4cc 425 parenthold = 0;
EasyCAT 0:543d6784d4cc 426 }
EasyCAT 0:543d6784d4cc 427 }
EasyCAT 0:543d6784d4cc 428 }
EasyCAT 0:543d6784d4cc 429
EasyCAT 0:543d6784d4cc 430 return context->slavelist[0].hasdc;
EasyCAT 0:543d6784d4cc 431 }
EasyCAT 0:543d6784d4cc 432
EasyCAT 0:543d6784d4cc 433 #ifdef EC_VER1
EasyCAT 0:543d6784d4cc 434 void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
EasyCAT 0:543d6784d4cc 435 {
EasyCAT 0:543d6784d4cc 436 ecx_dcsync0(&ecx_context, slave, act, CyclTime, CyclShift);
EasyCAT 0:543d6784d4cc 437 }
EasyCAT 0:543d6784d4cc 438
EasyCAT 0:543d6784d4cc 439 void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, int32 CyclShift)
EasyCAT 0:543d6784d4cc 440 {
EasyCAT 0:543d6784d4cc 441 ecx_dcsync01(&ecx_context, slave, act, CyclTime0, CyclTime1, CyclShift);
EasyCAT 0:543d6784d4cc 442 }
EasyCAT 0:543d6784d4cc 443
EasyCAT 0:543d6784d4cc 444 boolean ec_configdc(void)
EasyCAT 0:543d6784d4cc 445 {
EasyCAT 0:543d6784d4cc 446 return ecx_configdc(&ecx_context);
EasyCAT 0:543d6784d4cc 447 }
EasyCAT 0:543d6784d4cc 448 #endif