pt1588-SH7216 1
IEEE1588v2 Implementation for Renesas SH7216 Demo
|
00001 /* 00002 Copyright (c) 2010-2011 The Regents of the University of California. All rights 00003 reserved. 00004 00005 Permission is hereby granted, without written agreement and without license or 00006 royalty fees, to use, copy, modify, and distribute this software and its 00007 documentation for any purpose, provided that the above copyright notice and the 00008 following two paragraphs appear in all copies of this software. 00009 00010 IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR 00011 DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF 00012 THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF 00013 CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00014 00015 THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, 00016 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 00017 A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, 00018 AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION TO PROVIDE MAINTENANCE, 00019 SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS. 00020 */ 00021 00031 #include "ptp-protocol.h" 00032 #include <stdio.h> 00033 00034 PTPState ptp; 00036 UInteger16 zero = 0; 00040 void PTPActor(const Event *eventIn) { 00041 00042 // Handle input events. 00043 switch(eventIn->eventType) { 00044 00045 case EVENT_MSG_IN: 00046 eventMsgIn(eventIn); 00047 break; 00048 case EVENT_POWERUP: 00049 eventPowerup(); 00050 break; 00051 case EVENT_QUALIFICATION_TIMEOUT_EXPIRES: 00052 eventQualificationTimeoutExpires(); 00053 break; 00054 case EVENT_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES: 00055 eventAnnounceReceiptTimeoutExpires(eventIn); 00056 break; 00057 case EVENT_PERIODIC_ANNOUNCE: 00058 eventPeriodicAnnounce(); 00059 break; 00060 case EVENT_PERIODIC_SYNC: 00061 eventPeriodicSync(); 00062 break; 00063 case EVENT_DELAY_REQ: 00064 eventDelayReq(); 00065 break; 00066 case EVENT_FOLLOW_UP: 00067 eventFollowUp(); 00068 break; 00069 00070 // Currently not supported externally. 00071 /* 00072 case EVENT_INITIALIZE: 00073 eventInitialize(); 00074 break 00075 case EVENT_DESIGNATED_ENABLED: 00076 eventDesignatedEnabled(); 00077 break; 00078 case EVENT_DESIGNATED_DISABLED: 00079 eventDesignatedDisabled(); 00080 break; 00081 case EVENT_FAULT_CLEARED: 00082 eventFaultCleared(); 00083 break; 00084 case EVENT_FAULT_DETECTED: 00085 eventFaultDetected(); 00086 break; 00087 case EVENT_STATE_DECISION_EVENT: 00088 eventStateDecisionEvent(); 00089 break; 00090 case EVENT_SYNCHRONIZATION_FAULT: 00091 eventSynchronizationFault(); 00092 break; 00093 case EVENT_MASTER_CLOCK_SELECTED: 00094 eventMasterClockSelected(); 00095 break; 00096 */ 00097 } 00098 00099 } 00100 00104 void PTPInitialize() { 00105 00106 // Set up data structures and default values. 00107 ClockIdentity clockIdentity = CLOCKIDENTITY; 00108 00109 // DefaultDS Initialization (8.2.1). 00110 ptp.defaultDS.twoStepFlag = TWOSTEPFLAG; 00111 memcpy(ptp.defaultDS.clockIdentity, clockIdentity, 8); 00112 ptp.defaultDS.numberPorts = NUMBERPORTS; 00113 if(SLAVEONLY) { 00114 ptp.defaultDS.clockQuality.clockClass = 255; 00115 } 00116 else { 00117 ptp.defaultDS.clockQuality.clockClass = CLOCKCLASS; 00118 } 00119 ptp.defaultDS.clockQuality.clockAccuracy = CLOCKACCURACY; 00120 ptp.defaultDS.clockQuality.offsetScaledLogVariance = 00121 OFFSETSCALEDLOGVARIANCE; 00122 ptp.defaultDS.priority1 = PRIORITY1; 00123 ptp.defaultDS.priority2 = PRIORITY2; 00124 ptp.defaultDS.domainNumber = DOMAINNUMBER; 00125 ptp.defaultDS.slaveOnly = SLAVEONLY; 00126 00127 // CurrentDS Initialization (8.2.2). 00128 ptp.currentDS.stepsRemoved = 0; 00129 ptp.currentDS.offsetFromMaster.scaledNanoseconds = 0; 00130 ptp.currentDS.meanPathDelay.scaledNanoseconds = MEANPATHDELAY; 00131 ptp.currentDS.lastOffsetFromMaster.scaledNanoseconds = 0; 00132 00133 // ParentDS Initialization (8.2.3). 00134 memcpy(ptp.parentDS.parentPortIdentity.clockIdentity, 00135 ptp.defaultDS.clockIdentity, 8); 00136 ptp.parentDS.parentPortIdentity.portNumber = 0; 00137 ptp.parentDS.parentStats = FALSE; 00138 ptp.parentDS.observedParentOffsetScaledLogVariance = 0xFFFF; 00139 ptp.parentDS.observedParentClockPhaseChangeRate = 0x7FFFFFFF; 00140 memcpy(ptp.parentDS.grandmasterIdentity, ptp.defaultDS.clockIdentity, 8); 00141 ptp.parentDS.grandmasterClockQuality = ptp.defaultDS.clockQuality; 00142 ptp.parentDS.priority1 = ptp.defaultDS.priority1; 00143 ptp.parentDS.priority2 = ptp.defaultDS.priority2; 00144 00145 // TimeProperties Initialization (8.2.4). 00146 ptp.timePropertiesDS.currentUtcOffset = CURRENTUTCOFFSET; 00147 ptp.timePropertiesDS.currentUtcOffsetValid = CURRENTUTCOFFSETVALID; 00148 ptp.timePropertiesDS.leap59 = LEAP59; 00149 ptp.timePropertiesDS.leap61 = LEAP61; 00150 ptp.timePropertiesDS.timeTraceable = TIMETRACEABLE; 00151 ptp.timePropertiesDS.frequencyTraceable = FREQUENCYTRACEABLE; 00152 ptp.timePropertiesDS.ptpTimescale = PTPTIMESCALE; 00153 ptp.timePropertiesDS.timeSource = TIMESOURCE; 00154 00155 // PortDS Initialization (8.2.5). 00156 ptp.portDS.portIdentity.portNumber = 1; 00157 memcpy(ptp.portDS.portIdentity.clockIdentity, clockIdentity, 8); 00158 ptp.portDS.portState = INITIALIZING; 00159 ptp.portDS.logMinDelayReqInterval = LOGMINDELAYREQINTERVAL; 00160 ptp.portDS.peerMeanPathDelay.scaledNanoseconds = 0; 00161 ptp.portDS.logAnnounceInterval = LOGANNOUNCEINTERVAL; 00162 ptp.portDS.announceReceiptTimeout = ANNOUNCERECEIPTTIMEOUT; 00163 ptp.portDS.logSyncInterval = LOGSYNCINTERVAL; 00164 ptp.portDS.delayMechanism = E2E; 00165 ptp.portDS.versionNumber = VERSIONPTP; 00166 00167 // SequenceId Initialization. 00168 ptp.sequenceIdSync = 0; 00169 ptp.sequenceIdDelayReq = 0; 00170 ptp.sequenceIdAnnounce = 0; 00171 ptp.sequenceIdManagement = 0; 00172 00173 // Synchronization algorithm initialization. 00174 ptp.syncEventTimeInterval.scaledNanoseconds = 0; 00175 ptp.delayEventTimeInterval.scaledNanoseconds = 0; 00176 ptp.lastCorrectionField = 0; 00177 ptp.rateAdjFreq = 0; 00178 ptp.rateAdjFreqAcc = 0; 00179 ptp.rateAdjOffset = 0; 00180 ptp.rateAdjOffsetAcc = 0; 00181 00182 // This allows old timeout events to be ignored when they fire the actor. 00183 ptp.announceReceiptTimeoutExpiresID = 0; 00184 00185 // Current index for foreignMasterDS. 00186 ptp.foreignMasterDSIndex = 0; 00187 00188 ptp.sameMaster = TRUE; 00189 00190 // Start periodic announce interval. This periodic firing is still 00191 // used when in slave mode and Announce messages are not being sent. 00192 eventPeriodicAnnounce(); 00193 00194 } 00195 00203 void computeMeanPathDelay() { 00204 00205 // Represent difference in timestamps as a time interval 00206 // (scaled nanoseconds as defined by standard). 00207 toTimeInterval(&ptp.delayEventEgressTimestamp, 00208 &ptp.delayEventIngressTimestamp, 00209 &ptp.delayEventTimeInterval); 00210 00211 // Use correction field from Delay_Resp message. 00212 ptp.delayEventTimeInterval.scaledNanoseconds -= ptp.lastCorrectionField; 00213 00214 // Calculate meanPathDelay. 00215 ptp.currentDS.meanPathDelay.scaledNanoseconds = 00216 (ptp.syncEventTimeInterval.scaledNanoseconds >> 1) + 00217 (ptp.delayEventTimeInterval.scaledNanoseconds >> 1); 00218 00219 // Take into account clock changes between Sync Event and Delay Event. 00220 // Correct for clock step or rate adjust to correct offset 00221 // (currently ignores rate adjust to correct drift). 00222 if(ptp.lastAdjType == STEP || ptp.lastAdjType == STEP_2) { 00223 ptp.currentDS.meanPathDelay.scaledNanoseconds -= 00224 (ptp.currentDS.offsetFromMaster.scaledNanoseconds >> 1); 00225 } else if(ptp.lastAdjType == RATE) { 00226 } 00227 00228 } 00229 00233 void computeOffsetFromMaster() { 00234 00235 // Save last offsetFromMaster. 00236 ptp.currentDS.lastOffsetFromMaster.scaledNanoseconds = 00237 ptp.currentDS.offsetFromMaster.scaledNanoseconds; 00238 00239 // Represent difference in timestamps as a time interval 00240 // (scaled nanoseconds as defined by standard). 00241 toTimeInterval(&ptp.syncEventEgressTimestamp, 00242 &ptp.syncEventIngressTimestamp, 00243 &ptp.syncEventTimeInterval); 00244 00245 // Use correction field from Sync (or Follow_Up) message. 00246 ptp.syncEventTimeInterval.scaledNanoseconds -= ptp.lastCorrectionField; 00247 00248 // Calculate offsetFromMaster. 00249 ptp.currentDS.offsetFromMaster.scaledNanoseconds = 00250 ptp.syncEventTimeInterval.scaledNanoseconds - 00251 ptp.currentDS.meanPathDelay.scaledNanoseconds; 00252 00253 } 00254 00268 void adjustClock() { 00269 00270 Integer64 offset, freqAdj, offsetAdj; 00271 00272 // Check if within cutoff. 00273 // Get offset in ns. 00274 offset = (ptp.currentDS.offsetFromMaster.scaledNanoseconds >> 16); 00275 if((offset > CUTOFF) || (offset < -CUTOFF)) { 00276 00277 // Slave clock is too far seperated from master clock to be able to 00278 // syncrhronize solely with frequency adjustments, so step the clock. 00279 stepClockTimeInterval(&ptp.currentDS.offsetFromMaster); 00280 00281 // Fix timestamps. 00282 fixTimestamps(&ptp.currentDS.offsetFromMaster); 00283 00284 // Set adjustment type. 00285 ptp.lastAdjType = STEP; 00286 00287 } else { 00288 00289 // Offset (in ns). 00290 offsetAdj = offset; 00291 00292 // Change in offset (in ns). 00293 if(ptp.lastAdjType == STEP || ptp.lastAdjType == STEP_2) { 00294 // Clock was stepped so change in offset not valid. 00295 freqAdj = offsetAdj; 00296 } else { 00297 freqAdj = offset - 00298 (ptp.currentDS.lastOffsetFromMaster.scaledNanoseconds >> 16); 00299 } 00300 00301 // Convert to units of 100ps/s. 00302 // Use the time until next Sync message. 00303 if(ptp.portDS.logSyncInterval > 0) { 00304 offsetAdj = (-10*offsetAdj) >> ptp.portDS.logSyncInterval; 00305 freqAdj = (-10*freqAdj) >> ptp.portDS.logSyncInterval; 00306 } else { 00307 offsetAdj = (-10*offsetAdj) << (-ptp.portDS.logSyncInterval); 00308 freqAdj = (-10*freqAdj) << (-ptp.portDS.logSyncInterval); 00309 } 00310 00311 if(ptp.lastAdjType == STEP) { 00312 00313 // When clock is stepped, the mean path delay is most likely not 00314 // correctly known until after (For example, when adjustClock() 00315 // is first called). 00316 00317 // Now step the clock again with the valid delay. 00318 stepClockTimeInterval(&ptp.currentDS.offsetFromMaster); 00319 00320 // Fix timestamps. 00321 fixTimestamps(&ptp.currentDS.offsetFromMaster); 00322 00323 // Set adjustment rates. 00324 ptp.rateAdjFreq = freqAdj; 00325 ptp.rateAdjFreqAcc = 0; 00326 ptp.rateAdjOffset = 0; 00327 ptp.rateAdjOffsetAcc = 0; 00328 rateAdjustClock(ptp.rateAdjFreq); 00329 00330 // Set adjustment type. 00331 ptp.lastAdjType = STEP_2; 00332 00333 } else { 00334 00335 // rateAdjOffset is used as a temporary rate adjustment to fix 00336 // offset, so remove this effect from change in offset since last 00337 // sync message. 00338 freqAdj += ptp.rateAdjOffset; 00339 00340 // Since last synchronization, any change in offset should be due 00341 // solely to clock drift (After effect of rateAdjOffset is 00342 // corrected). rateAdjFreq is updated to attempt to 00343 // adjust frequency to lock to master clock. Even if this 00344 // synchronizes the clock rate of master and slave, there will still 00345 // be an offset, so rateAdjOffset temporarily adjusts the rate to 00346 // correct for this offset by the time of the next adjustClock(). 00347 00348 // Set adjustment rates. 00349 ptp.rateAdjFreq += FREQ_P(freqAdj); 00350 ptp.rateAdjFreqAcc += FREQ_I(freqAdj); 00351 ptp.rateAdjOffset = OFFSET_P(offsetAdj); 00352 ptp.rateAdjOffsetAcc = OFFSET_I(offsetAdj); 00353 00354 rateAdjustClock(ptp.rateAdjFreq + ptp.rateAdjFreqAcc + 00355 ptp.rateAdjOffset + ptp.rateAdjOffsetAcc); 00356 00357 // Set adjustement type. 00358 ptp.lastAdjType = RATE; 00359 00360 } 00361 00362 } 00363 00364 printStatus(); 00365 00366 } 00367 00373 void generateAnnounceReceiptTimeout() { 00374 00375 Event event; 00376 00377 //DEBUG_PRINTF("Announce Receipt Timeout\n"); 00378 00379 // This will cause all past announceReceiptTimeouts to be ignored. 00380 ptp.announceReceiptTimeoutExpiresID++; 00381 00382 // Set correct event timestamp. 00383 getClock(&event.eventTimestamp); 00384 //TODO: currently doesn't add random number (9.2.6.11) 00385 (event.eventTimestamp.secondsField.lsb) += 00386 ptp.portDS.announceReceiptTimeout * 00387 (1 << ptp.portDS.logAnnounceInterval); 00388 00389 // Assign event type. 00390 event.eventType = EVENT_ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES; 00391 00392 // Assign ID to event. 00393 event.eventDataPointer = (void *)malloc(sizeof(UInteger16)); 00394 *((UInteger16 *)event.eventDataPointer) = 00395 ptp.announceReceiptTimeoutExpiresID; 00396 00397 // Add event. 00398 addFutureEvent(&event); 00399 00400 } 00401 00407 void generateQualificationTimeout(UInteger8 N) { 00408 00409 Event event; 00410 00411 // Set correct event timestamp. 00412 getClock(&event.eventTimestamp); 00413 (event.eventTimestamp.secondsField.lsb) += 00414 N * (1 << ptp.portDS.logAnnounceInterval); 00415 00416 // Assign event type. 00417 event.eventType = EVENT_QUALIFICATION_TIMEOUT_EXPIRES; 00418 00419 // Add event. 00420 addFutureEvent(&event); 00421 00422 } 00423 00426 void eventMsgIn(const Event *eventIn) { 00427 00428 UInteger8* msg = (UInteger8 *)eventIn->eventDataPointer; 00429 UInteger4 messageType = msg[0] & 0x0F; 00430 00431 switch(messageType) { 00432 00433 case SYNC: 00434 syncMsgIn(msg); 00435 break; 00436 case FOLLOW_UP: 00437 followUpMsgIn(msg); 00438 break; 00439 case DELAY_REQ: 00440 delayReqMsgIn(msg); 00441 break; 00442 case DELAY_RESP: 00443 delayRespMsgIn(msg); 00444 break; 00445 case ANNOUNCE: 00446 announceMsgIn(msg); 00447 break; 00448 case MANAGEMENT: 00449 managementMsgIn(msg); 00450 00451 } 00452 00453 } 00454 00456 void eventPowerup() { 00457 00458 // Figure 23/24 (9.2.5). 00459 ptp.portDS.portState = INITIALIZING; 00460 stateInitializing(); 00461 00462 } 00463 00465 void eventInitialize() { 00466 00467 // Figure 23/24 (9.2.5). 00468 ptp.portDS.portState = INITIALIZING; 00469 stateInitializing(); 00470 00471 } 00472 00474 void eventDesignatedEnabled() { 00475 00476 // Figure 23/24 (9.2.5). 00477 if(ptp.portDS.portState == DISABLED) { 00478 ptp.portDS.portState = INITIALIZING; 00479 stateInitializing(); 00480 } 00481 } 00482 00484 void eventDesignatedDisabled() { 00485 00486 // Figure 23/24 (9.2.5). 00487 if(ptp.portDS.portState == LISTENING || 00488 ptp.portDS.portState == UNCALIBRATED || 00489 ptp.portDS.portState == SLAVE || 00490 ptp.portDS.portState == PRE_MASTER || 00491 ptp.portDS.portState == MASTER || 00492 ptp.portDS.portState == PASSIVE || 00493 ptp.portDS.portState == FAULTY) { 00494 00495 ptp.portDS.portState = DISABLED; 00496 stateDisabled(); 00497 } 00498 00499 } 00500 00502 void eventFaultCleared() { 00503 00504 // Figure 23/24 (9.2.5). 00505 if(ptp.portDS.portState == FAULTY) { 00506 ptp.portDS.portState = INITIALIZING; 00507 stateInitializing(); 00508 } 00509 00510 } 00511 00513 void eventFaultDetected() { 00514 00515 // Figure 23/24 (9.2.5). 00516 if(ptp.portDS.portState == LISTENING || 00517 ptp.portDS.portState == UNCALIBRATED || 00518 ptp.portDS.portState == SLAVE || 00519 ptp.portDS.portState == PRE_MASTER || 00520 ptp.portDS.portState == MASTER || 00521 ptp.portDS.portState == PASSIVE || 00522 ptp.portDS.portState == FAULTY) { 00523 00524 ptp.portDS.portState = FAULTY; 00525 stateFaulty(); 00526 00527 } 00528 00529 } 00530 00534 void eventStateDecisionEvent() { 00535 00536 UInteger8 i,j; 00537 Integer8 best; 00538 ClockDataSet a,b; 00539 00540 // Cannot occur in INITIALIZING state. 00541 if(ptp.portDS.portState == INITIALIZING) 00542 return; 00543 00544 // Figure 25 (9.2.6.8). 00545 00546 // Find Erbest (9.3.2.3). 00547 best = -1; 00548 00549 // Check if any foreign clocks have been detected. 00550 if(ptp.foreignMasterDSIndex > 0) { 00551 00552 // Find first foreign master with qualified announce message. 00553 for(i = 0; i < ptp.foreignMasterDSIndex; i++) { 00554 00555 // Check if announce message is qualified (9.3.2.5). 00556 if(isQualified(&ptp.foreignMasterDS[i]) == TRUE) { 00557 toDataSetFromMsg(&a, &ptp.foreignMasterDS[i].msg); 00558 best = i; 00559 break; 00560 } 00561 00562 } 00563 00564 // Compare against any remaining foreign masters with qualified 00565 // announce messages. 00566 for(i = i + 1; i < ptp.foreignMasterDSIndex; i++) { 00567 00568 // Check if announce message is qualified (9.3.2.5). 00569 if(isQualified(&ptp.foreignMasterDS[i]) == TRUE) { 00570 toDataSetFromMsg(&b, &ptp.foreignMasterDS[i].msg); 00571 00572 // Compare clocks. 00573 j = compareDataSet(&a, &b); 00574 00575 // Check if b is better than a. 00576 if(j == DATA_SET_B || j == DATA_SET_B_T) { 00577 a = b; 00578 best = i; 00579 } 00580 00581 } 00582 00583 } 00584 00585 } 00586 00587 // At this point, 'best' contains the index value of the best 00588 // foreign master (or -1 if none exist), and 'a' the clock info. 00589 if(best != -1) { 00590 00591 // Make 'best' the only foreign master in data set (9.3.2.3b). 00592 if(best != 0) { 00593 ptp.foreignMasterDS[0] = ptp.foreignMasterDS[best]; 00594 } 00595 ptp.foreignMasterDSIndex = 1; 00596 00597 } 00598 00599 // Figure 26 (9.3.3). 00600 if(best == -1 && ptp.portDS.portState == LISTENING) 00601 return; 00602 00603 // Local clock. 00604 toDataSetFromLocal(&b); 00605 00606 if(ptp.defaultDS.clockQuality.clockClass <= 127) { 00607 00608 if(best == -1) { 00609 00610 // BMC_MASTER (D0) M1 00611 updateDataSet(STATE_DECISION_CODE_M1, NULL); 00612 i = BMC_MASTER; 00613 00614 } else { 00615 00616 j = compareDataSet(&a, &b); 00617 00618 if(j == DATA_SET_B || j == DATA_SET_B_T) { 00619 00620 // BMC_MASTER (D0) M1 00621 updateDataSet(STATE_DECISION_CODE_M1, NULL); 00622 i = BMC_MASTER; 00623 00624 } else { 00625 00626 // BMC_PASSIVE (Erbest) P1 00627 updateDataSet(STATE_DECISION_CODE_P1, NULL); 00628 i = BMC_PASSIVE; 00629 00630 } 00631 00632 } 00633 00634 } else { 00635 00636 if(best == -1) { 00637 00638 // BMC_MASTER (D0) M2 00639 updateDataSet(STATE_DECISION_CODE_M2, NULL); 00640 i = BMC_MASTER; 00641 00642 } else { 00643 00644 j = compareDataSet(&a,&b); 00645 00646 if(j == DATA_SET_B || j == DATA_SET_B_T) { 00647 00648 // BMC_MASTER (D0) M2 00649 updateDataSet(STATE_DECISION_CODE_M2, NULL); 00650 i = BMC_MASTER; 00651 00652 } else { 00653 00654 // BMC_SLAVE 00655 updateDataSet(STATE_DECISION_CODE_S1, 00656 &ptp.foreignMasterDS[best].msg); 00657 i = BMC_SLAVE; 00658 00659 } 00660 00661 } 00662 00663 } 00664 00665 // Update state (Figure 23/24) (9.2.5). 00666 if((ptp.portDS.portState == LISTENING || 00667 ptp.portDS.portState == UNCALIBRATED || 00668 ptp.portDS.portState == SLAVE || 00669 ptp.portDS.portState == PRE_MASTER || 00670 ptp.portDS.portState == PASSIVE) && 00671 i == BMC_MASTER && 00672 ptp.defaultDS.slaveOnly == FALSE) { 00673 00674 ptp.portDS.portState = PRE_MASTER; 00675 statePreMaster(); 00676 return; 00677 00678 } 00679 00680 if((ptp.portDS.portState == LISTENING || 00681 ptp.portDS.portState == UNCALIBRATED || 00682 ptp.portDS.portState == PRE_MASTER || 00683 ptp.portDS.portState == MASTER || 00684 ptp.portDS.portState == PASSIVE) && 00685 i == BMC_SLAVE) { 00686 00687 ptp.portDS.portState = UNCALIBRATED; 00688 stateUncalibrated(); 00689 return; 00690 00691 } 00692 00693 if((ptp.portDS.portState == LISTENING || 00694 ptp.portDS.portState == UNCALIBRATED || 00695 ptp.portDS.portState == SLAVE || 00696 ptp.portDS.portState == PRE_MASTER || 00697 ptp.portDS.portState == MASTER || 00698 ptp.portDS.portState == PASSIVE) && 00699 i == BMC_PASSIVE && 00700 ptp.defaultDS.slaveOnly == FALSE) { 00701 00702 ptp.portDS.portState = PASSIVE; 00703 statePassive(); 00704 return; 00705 00706 } 00707 00708 if(ptp.portDS.portState == SLAVE && 00709 i == BMC_SLAVE && ptp.sameMaster == FALSE) { 00710 00711 ptp.portDS.portState = UNCALIBRATED; 00712 stateUncalibrated(); 00713 return; 00714 00715 } 00716 00717 if(ptp.defaultDS.slaveOnly == TRUE && 00718 (ptp.portDS.portState == LISTENING || 00719 ptp.portDS.portState == UNCALIBRATED || 00720 ptp.portDS.portState == SLAVE) && 00721 (i == BMC_MASTER || i == BMC_PASSIVE)) { 00722 00723 ptp.portDS.portState = LISTENING; 00724 stateListening(); 00725 00726 } 00727 } 00728 00730 void eventQualificationTimeoutExpires() { 00731 00732 // Figure 23/24 (9.2.5). 00733 if(ptp.portDS.portState == PRE_MASTER) { 00734 00735 ptp.portDS.portState = MASTER; 00736 stateMaster(); 00737 00738 } 00739 00740 } 00741 00745 void eventAnnounceReceiptTimeoutExpires(const Event *eventIn) { 00746 00747 // Figure 23/24 (9.2.5). 00748 UInteger16 Id = *((UInteger16 *)eventIn->eventDataPointer); 00749 00750 // Free space allocated to storing announceReceiptTimeoutExpiresID. 00751 free(eventIn->eventDataPointer); 00752 00753 // Check if event is still relevant. 00754 if(Id == ptp.announceReceiptTimeoutExpiresID) { 00755 00756 if(ptp.portDS.portState == LISTENING || 00757 ptp.portDS.portState == UNCALIBRATED || 00758 ptp.portDS.portState == SLAVE || 00759 ptp.portDS.portState == PASSIVE) { 00760 00761 00762 if(ptp.defaultDS.slaveOnly == TRUE) { 00763 00764 ptp.portDS.portState = LISTENING; 00765 stateListening(); 00766 return; 00767 00768 } 00769 00770 // Update port's data sets to MASTER. (9.2.6.11) 00771 updateDataSet(STATE_DECISION_CODE_M1, NULL); 00772 ptp.portDS.portState = MASTER; 00773 stateMaster(); 00774 00775 } else { 00776 00777 // Restart AnnouceReceiptTimeout. (b) 00778 generateAnnounceReceiptTimeout(); 00779 00780 } 00781 00782 } 00783 00784 } 00785 00787 void eventSynchronizationFault() { 00788 00789 // Figure 23/24. 00790 if(ptp.portDS.portState == SLAVE) { 00791 00792 ptp.portDS.portState = UNCALIBRATED; 00793 stateUncalibrated(); 00794 00795 } 00796 00797 } 00798 00800 void eventMasterClockSelected() { 00801 00802 // Figure 23/24. 00803 if(ptp.portDS.portState == UNCALIBRATED) { 00804 00805 ptp.portDS.portState = SLAVE; 00806 stateSlave(); 00807 00808 } 00809 00810 } 00811 00815 void eventPeriodicAnnounce() { 00816 00817 Event eventOut; 00818 00819 if(ptp.portDS.portState == MASTER) { 00820 00821 // Send Announce message. 00822 sendAnnounceMsg(&ptp); 00823 00824 } 00825 00826 // Set time of next Announce message. 00827 getClock(&eventOut.eventTimestamp); 00828 (eventOut.eventTimestamp.secondsField.lsb) += 00829 (1 << ptp.portDS.logAnnounceInterval); 00830 eventOut.eventType = EVENT_PERIODIC_ANNOUNCE; 00831 addFutureEvent(&eventOut); 00832 00833 // Run BMC algorithm. 00834 eventStateDecisionEvent(); 00835 00836 printStatus(); 00837 00838 } 00839 00843 void eventPeriodicSync() { 00844 00845 Event eventOut; 00846 TimeInterval timeInterval; 00847 00848 if(ptp.portDS.portState == MASTER) { 00849 00850 // Send Sync message. 00851 sendSyncMsg(&ptp); 00852 00853 getClock(&eventOut.eventTimestamp); 00854 00855 // Schedule Follow_Up if required. 00856 if(ptp.defaultDS.twoStepFlag == TRUE) { 00857 00858 eventOut.eventType = EVENT_FOLLOW_UP; 00859 addFutureEvent(&eventOut); 00860 00861 } 00862 00863 // Set time of next Sync message. 00864 00865 // Set initially to 1s. 00866 timeInterval.scaledNanoseconds = 1000000000; 00867 timeInterval.scaledNanoseconds = timeInterval.scaledNanoseconds << 16; 00868 00869 // Modify 1s to correct time based on logSyncInterval. 00870 if(ptp.portDS.logSyncInterval > 0) { 00871 timeInterval.scaledNanoseconds = 00872 timeInterval.scaledNanoseconds << ptp.portDS.logSyncInterval; 00873 } else { 00874 timeInterval.scaledNanoseconds = 00875 timeInterval.scaledNanoseconds >> (-ptp.portDS.logSyncInterval); 00876 } 00877 00878 addTimeIntervalToTimestamp(&timeInterval, &eventOut.eventTimestamp); 00879 eventOut.eventType = EVENT_PERIODIC_SYNC; 00880 addFutureEvent(&eventOut); 00881 00882 } 00883 00884 } 00885 00887 void eventDelayReq() { 00888 00889 // Send Delay Req message. 00890 sendDelayReqMsg(&ptp); 00891 00892 } 00893 00895 void eventFollowUp() { 00896 00897 // Get timestamp of Sync message. 00898 getSyncEventEgressTimestamp(&ptp); 00899 00900 // Send Follow Up message. 00901 sendFollowUpMsg(&ptp); 00902 00903 } 00904 00908 void stateInitializing() { 00909 00910 //DEBUG_PRINTF("State: INITIALIZING\n"); 00911 00912 // Initialize data sets. 00913 PTPInitialize(); 00914 00915 // Stop AnnounceReceiptTimeout (9.2.6.11). 00916 ptp.announceReceiptTimeoutExpiresID++; 00917 00918 // Now that the port has been initialized, 00919 // transition to LISTENING state. (9.2.5, Fig 23/24) 00920 ptp.portDS.portState = LISTENING; 00921 stateListening(); 00922 00923 } 00924 00926 void stateFaulty() { 00927 00928 //DEBUG_PRINTF("State: FAULTY\n"); 00929 00930 // Stop AnnounceReceiptTimeout (9.2.6.11). 00931 ptp.announceReceiptTimeoutExpiresID++; 00932 00933 } 00934 00936 void stateDisabled() { 00937 00938 //DEBUG_PRINTF("State: DISABLED\n"); 00939 00940 // Stop AnnounceReceiptTimeout (9.2.6.11). 00941 ptp.announceReceiptTimeoutExpiresID++; 00942 00943 } 00944 00946 void stateListening() { 00947 00948 //DEBUG_PRINTF("State: LISTENING\n"); 00949 00950 // Restart AnnouceReceiptTimeout (9.2.6.11c). 00951 generateAnnounceReceiptTimeout(); 00952 00953 } 00954 00956 void statePreMaster() { 00957 00958 //DEBUG_PRINTF("State: PRE_MASTER\n"); 00959 00960 // Stop AnnounceReceiptTimeout. 00961 ptp.announceReceiptTimeoutExpiresID++; 00962 00963 // Start QualificationTimeout. 00964 // M3 not supported (9.2.6.10) 00965 generateQualificationTimeout(0); 00966 00967 } 00968 00971 void stateMaster() { 00972 00973 //DEBUG_PRINTF("State: MASTER\n"); 00974 00975 // Stop AnnounceReceiptTimeout. 00976 ptp.announceReceiptTimeoutExpiresID++; 00977 00978 // Periodically send sync message. 00979 eventPeriodicSync(); 00980 00981 } 00982 00984 void statePassive() { 00985 00986 //DEBUG_PRINTF("State: PASSIVE\n"); 00987 00988 // Restart AnnouceReceiptTimeout (9.2.6.11c). 00989 generateAnnounceReceiptTimeout(); 00990 00991 } 00992 00994 void stateUncalibrated() { 00995 00996 //DEBUG_PRINTF("State: UNCALIBRATED\n"); 00997 00998 // Restart AnnouceReceiptTimeout (9.2.6.11c). 00999 generateAnnounceReceiptTimeout(); 01000 01001 eventMasterClockSelected(); 01002 01003 } 01004 01006 void stateSlave() { 01007 01008 //DEBUG_PRINTF("State: SLAVE\n"); 01009 01010 // Restart AnnouceReceiptTimeout (9.2.6.11c). 01011 generateAnnounceReceiptTimeout(); 01012 01013 } 01014 01016 void announceMsgIn(UInteger8* msg) { 01017 01018 AnnounceMsg announceMsg; 01019 UInteger8 i; 01020 01021 // Receipt of Announce message logic (9.5.3, Figure 29). 01022 if(ptp.portDS.portState == INITIALIZING || 01023 ptp.portDS.portState == DISABLED || 01024 ptp.portDS.portState == FAULTY) { 01025 return; 01026 } 01027 01028 // Parse the received Announce message. 01029 readAnnounceMsg(&announceMsg); 01030 01031 // Break if alternateMasterFlag is True. 01032 if((announceMsg.header.flagField[0] & 0x01) == TRUE) { 01033 return; 01034 } 01035 01036 //TODO: announce receipt timeout (d) (PASSIVE state) (9.2.6.11) 01037 01038 // Check if from current master. 01039 if(comparePortIdentity(&announceMsg.header.sourcePortIdentity, 01040 &ptp.parentDS.parentPortIdentity) == TRUE) { 01041 01042 if(ptp.portDS.portState == UNCALIBRATED || 01043 ptp.portDS.portState == SLAVE) { 01044 01045 // Restart AnnouceReceiptTimeout. (a) 01046 generateAnnounceReceiptTimeout(); 01047 01048 } 01049 01050 //TODO: if SLAVE, update data sets? 01051 } 01052 01053 // Add or update foreignMasterDS. 01054 for(i = 0; i < ptp.foreignMasterDSIndex; i++) { 01055 01056 if(comparePortIdentity(&announceMsg.header.sourcePortIdentity, 01057 &ptp.foreignMasterDS[i].foreignMasterPortIdentity) == TRUE) { 01058 01059 // Already exists. 01060 01061 // Not qualified if not most recent. 01062 if(announceMsg.header.sequenceId < 01063 ptp.foreignMasterDS[i].msg.header.sequenceId) { 01064 break; 01065 } 01066 01067 // Update timestamps of announce messages. 01068 ptp.foreignMasterDS[i].announceMessageTimestamps[0] = 01069 ptp.foreignMasterDS[i].announceMessageTimestamps[1]; 01070 getClock(&ptp.foreignMasterDS[i].announceMessageTimestamps[1]); 01071 01072 // Update to latest announce message. 01073 ptp.foreignMasterDS[i].msg = announceMsg; 01074 01075 break; 01076 01077 } 01078 } 01079 01080 if(i == ptp.foreignMasterDSIndex) { 01081 01082 // Create new record. 01083 01084 // Prevent overflow of foreignMasterDS. 01085 if(ptp.foreignMasterDSIndex >= FOREIGNMASTERDSSIZE) { 01086 ptp.foreignMasterDSIndex = FOREIGNMASTERDSSIZE - 1; 01087 } 01088 01089 // Add to foreignMasterDS. 01090 // Initialize. 01091 ptp.foreignMasterDS[ptp.foreignMasterDSIndex]. 01092 foreignMasterPortIdentity = announceMsg.header.sourcePortIdentity; 01093 ptp.foreignMasterDS[ptp.foreignMasterDSIndex]. 01094 foreignMasterAnnounceMessages = 0; 01095 ptp.foreignMasterDS[ptp.foreignMasterDSIndex]. 01096 msg = announceMsg; 01097 ptp.foreignMasterDS[ptp.foreignMasterDSIndex]. 01098 announceMessageTimestamps[0].secondsField.msb = 0; 01099 ptp.foreignMasterDS[ptp.foreignMasterDSIndex]. 01100 announceMessageTimestamps[0].secondsField.lsb = 0; 01101 ptp.foreignMasterDS[ptp.foreignMasterDSIndex]. 01102 announceMessageTimestamps[0].nanosecondsField = 0; 01103 getClock(&ptp.foreignMasterDS[ptp.foreignMasterDSIndex]. 01104 announceMessageTimestamps[1]); 01105 ptp.foreignMasterDSIndex++; 01106 01107 } 01108 01109 } 01110 01112 void syncMsgIn(UInteger8* msg) { 01113 01114 SyncMsg syncMsg; 01115 Event eventOut; 01116 Integer64 ns; 01117 TimeInterval timeInterval; 01118 01119 // Receipt of Sync message logic (9.5.4, Figure 30). 01120 if(ptp.portDS.portState == INITIALIZING || 01121 ptp.portDS.portState == DISABLED || 01122 ptp.portDS.portState == FAULTY || 01123 !(ptp.portDS.portState == SLAVE || 01124 ptp.portDS.portState == UNCALIBRATED)) { 01125 clearIngressTimestamp(); 01126 return; 01127 } 01128 01129 // Parse the received Sync message. 01130 readSyncMsg(&syncMsg); 01131 01132 // Make sure it is from current master clock. 01133 if(comparePortIdentity(&syncMsg.header.sourcePortIdentity, 01134 &ptp.parentDS.parentPortIdentity) == FALSE) { 01135 clearIngressTimestamp(); 01136 return; 01137 } 01138 01139 // Get syncEventIngressTimestamp. 01140 if(getSyncEventIngressTimestamp(&ptp, syncMsg.header.sequenceId) == FALSE) { 01141 return; 01142 } 01143 01144 01145 // Set time to send Delay_Req (9.5.11.2). 01146 getClock(&eventOut.eventTimestamp); 01147 ns = ((ptp.portDS.logMinDelayReqInterval + 1) << 1) * (1000000000); 01148 ns = (ns * (rand() % ((1 << DELAYREQDIV) + 1))) >> DELAYREQDIV; 01149 timeInterval.scaledNanoseconds = ns << 16; 01150 addTimeIntervalToTimestamp(&timeInterval, &eventOut.eventTimestamp); 01151 eventOut.eventType = EVENT_DELAY_REQ; 01152 addFutureEvent(&eventOut); 01153 01154 // Syncronize local clock if twoStepFlag is false. 01155 if((syncMsg.header.flagField[0] & 0x02) == 0) { 01156 01157 // Accurate timestamp is in Sync Message. 01158 ptp.syncEventEgressTimestamp = syncMsg.originTimestamp; 01159 01160 // Get Correction Field 01161 ptp.lastCorrectionField = syncMsg.header.correctionField 01162 + DELAY_ASYMMETRY; 01163 01164 // Calculate offsetFromMaster. 01165 computeOffsetFromMaster(); 01166 01167 // Synchronize! 01168 adjustClock(); 01169 01170 } 01171 01172 // Store sync message information. 01173 ptp.lastSyncMsgSourcePortIdentity = syncMsg.header.sourcePortIdentity; 01174 ptp.lastSyncMsgSequenceId = syncMsg.header.sequenceId; 01175 ptp.lastSyncMsgCorrectionField = syncMsg.header.correctionField; 01176 01177 } 01178 01180 void followUpMsgIn(UInteger8* msg) { 01181 01182 FollowUpMsg followUpMsg; 01183 01184 // Receipt of Follow_Up message logic (9.5.5, Figure 31). 01185 if(ptp.portDS.portState == INITIALIZING || 01186 ptp.portDS.portState == DISABLED || 01187 ptp.portDS.portState == FAULTY || 01188 !(ptp.portDS.portState == SLAVE || 01189 ptp.portDS.portState == UNCALIBRATED)) { 01190 01191 return; 01192 } 01193 01194 // Parse the received Follow_Up message. 01195 readFollowUpMsg(&followUpMsg); 01196 01197 // Make sure it is from current master clock. 01198 if(comparePortIdentity(&followUpMsg.header.sourcePortIdentity, 01199 &ptp.parentDS.parentPortIdentity) == FALSE) { 01200 return; 01201 } 01202 01203 // Make sure it is associated with the last sync message. 01204 if(comparePortIdentity(&followUpMsg.header.sourcePortIdentity, 01205 &ptp.lastSyncMsgSourcePortIdentity) == TRUE && 01206 followUpMsg.header.sequenceId == ptp.lastSyncMsgSequenceId) { 01207 01208 // Store accurate timestamp for sync message. 01209 ptp.syncEventEgressTimestamp = followUpMsg.preciseOriginTimestamp; 01210 01211 // Get Correction Field 01212 ptp.lastCorrectionField = ptp.lastSyncMsgCorrectionField + 01213 followUpMsg.header.correctionField + 01214 DELAY_ASYMMETRY; 01215 01216 // Calculate offsetFromMaster. 01217 computeOffsetFromMaster(); 01218 01219 // Synchronize! 01220 adjustClock(); 01221 01222 } 01223 01224 } 01225 01227 void delayReqMsgIn(UInteger8* msg) { 01228 01229 DelayReqMsg delayReqMsg; 01230 01231 // Receipt of Delay_Req message logic (9.5.6, Figure 32). 01232 if(ptp.portDS.portState == INITIALIZING || 01233 ptp.portDS.portState == DISABLED || 01234 ptp.portDS.portState == FAULTY || 01235 ptp.portDS.portState != MASTER) { 01236 clearIngressTimestamp(); 01237 return; 01238 } 01239 01240 // Parse the received Delay Req message. 01241 readDelayReqMsg(&delayReqMsg); 01242 01243 // Get timestamp. 01244 if(getDelayEventIngressTimestamp(&ptp, delayReqMsg.header.sequenceId) 01245 == FALSE) { 01246 return; 01247 } 01248 01249 // Send Delay Resp message. 01250 sendDelayRespMsg(&ptp, &delayReqMsg); 01251 01252 } 01253 01255 void delayRespMsgIn(UInteger8* msg) { 01256 01257 DelayRespMsg delayRespMsg; 01258 01259 // Receipt of Delay_Resp message logic (Figure 33). 01260 if(ptp.portDS.portState == INITIALIZING || 01261 ptp.portDS.portState == DISABLED || 01262 ptp.portDS.portState == FAULTY || 01263 !(ptp.portDS.portState == SLAVE || 01264 ptp.portDS.portState == UNCALIBRATED)) { 01265 return; 01266 } 01267 01268 // Parse the received Delay Resp mesage. 01269 readDelayRespMsg(&delayRespMsg); 01270 01271 // Check if Delay_Resp message is associated with most recent Delay_Req 01272 // message. 01273 if((comparePortIdentity(&delayRespMsg.header.sourcePortIdentity, 01274 &ptp.parentDS.parentPortIdentity) == FALSE) || 01275 (comparePortIdentity(&delayRespMsg.requestingPortIdentity, 01276 &ptp.portDS.portIdentity) == FALSE) || 01277 (delayRespMsg.header.sequenceId != ptp.sequenceIdDelayReq)) { 01278 return; 01279 } 01280 01281 // Get Delay Req message egress timestamp. 01282 getDelayEventEgressTimestamp(&ptp); 01283 01284 // Get Delay Req message ingress timestamp from Delay Resp message. 01285 ptp.delayEventIngressTimestamp = delayRespMsg.receiveTimestamp; 01286 01287 // Update 01288 ptp.portDS.logMinDelayReqInterval = delayRespMsg.header.logMessageInterval; 01289 01290 // Get Correction Field 01291 ptp.lastCorrectionField = delayRespMsg.header.correctionField; 01292 01293 // Calculate meanPathDelay. 01294 computeMeanPathDelay(); 01295 01296 } 01297 01299 void managementMsgIn(UInteger8* msg){ 01300 01301 ManagementMsg managementMsg; 01302 01303 readManagementMsg(&managementMsg); 01304 01305 //TODO: Implement management messages. 01306 01307 } 01308 01312 void toDataSetFromMsg(ClockDataSet *ds, AnnounceMsg *msg) { 01313 01314 ds->priority1 = &msg->grandmasterPriority1; 01315 ds->identity = &msg->grandmasterIdentity; 01316 ds->clockClass = &msg->grandmasterClockQuality.clockClass; 01317 ds->accuracy = &msg->grandmasterClockQuality.clockAccuracy; 01318 ds->offsetScaledLogVariance = 01319 &msg->grandmasterClockQuality.offsetScaledLogVariance; 01320 ds->priority2 = &msg->grandmasterPriority2; 01321 ds->stepsRemoved = &msg->stepsRemoved; 01322 ds->sender = &msg->header.sourcePortIdentity.clockIdentity; 01323 ds->receiver = &ptp.parentDS.parentPortIdentity.clockIdentity; 01324 ds->receiverPortNumber = &ptp.parentDS.parentPortIdentity.portNumber; 01325 01326 } 01327 01331 void toDataSetFromLocal(ClockDataSet *ds) { 01332 01333 ds->priority1 = &ptp.defaultDS.priority1; 01334 ds->identity = &ptp.defaultDS.clockIdentity; 01335 ds->clockClass = &ptp.defaultDS.clockQuality.clockClass; 01336 ds->accuracy = &ptp.defaultDS.clockQuality.clockAccuracy; 01337 ds->offsetScaledLogVariance = 01338 &ptp.defaultDS.clockQuality.offsetScaledLogVariance; 01339 ds->priority2 = &ptp.defaultDS.priority2; 01340 ds->stepsRemoved = &zero; 01341 ds->sender = &ptp.defaultDS.clockIdentity; 01342 ds->receiver = &ptp.defaultDS.clockIdentity; 01343 ds->receiverPortNumber = &zero; 01344 01345 01346 } 01347 01351 UInteger8 compareDataSet(const ClockDataSet *a, const ClockDataSet *b) { 01352 01353 UInteger8 i; 01354 01355 // Data comparison algorithm as in Figure 27 (9.3.4). 01356 i = memcmp(*a->identity, *b->identity, 8); 01357 if(i == 0) { 01358 // Data comparison algorithm as in Figure 28 (9.3.4). 01359 if(*a->stepsRemoved > *b->stepsRemoved + 1) { 01360 return DATA_SET_B; 01361 } else if(*a->stepsRemoved + 1 < *b->stepsRemoved) { 01362 return DATA_SET_A; 01363 } else if(*a->stepsRemoved > *b->stepsRemoved) { 01364 i = memcmp(*a->receiver, *a->sender, 8); 01365 if(i == 0) { 01366 return DATA_SET_ERROR_1; 01367 } else if (*a->receiver[i] < *a->sender[i]) { 01368 return DATA_SET_B; 01369 } else { 01370 return DATA_SET_B_T; 01371 } 01372 } else if(*a->stepsRemoved < *b->stepsRemoved) { 01373 i = memcmp(*b->receiver, *b->sender, 8); 01374 if(i == 0) { 01375 return DATA_SET_ERROR_1; 01376 } else if (*b->receiver[i] < *b->sender[i]) { 01377 return DATA_SET_A; 01378 } else { 01379 return DATA_SET_A_T; 01380 } 01381 } else { 01382 i = memcmp(*a->sender, *b->sender, 8); 01383 if(i == 0) { 01384 if(*a->receiverPortNumber > *b->receiverPortNumber) { 01385 return DATA_SET_B_T; 01386 } else if(*a->receiverPortNumber < *b->receiverPortNumber) { 01387 return DATA_SET_A_T; 01388 } else { 01389 return DATA_SET_ERROR_2; 01390 } 01391 } else if(*a->sender[i] > *b->sender[i]) { 01392 return DATA_SET_B_T; 01393 } else { 01394 return DATA_SET_A_T; 01395 } 01396 } 01397 } else if(*a->priority1 > *b->priority1) { 01398 return DATA_SET_B; 01399 } else if(*a->priority1 < *b->priority1) { 01400 return DATA_SET_A; 01401 } else if(*a->clockClass > *b->clockClass) { 01402 return DATA_SET_B; 01403 } else if(*a->clockClass < *b->clockClass) { 01404 return DATA_SET_A; 01405 } else if(*a->accuracy > *b->accuracy) { 01406 return DATA_SET_B; 01407 } else if(*a->accuracy < *b->accuracy) { 01408 return DATA_SET_A; 01409 } else if(*a->offsetScaledLogVariance > *b->offsetScaledLogVariance) { 01410 return DATA_SET_B; 01411 } else if(*a->offsetScaledLogVariance < *b->offsetScaledLogVariance) { 01412 return DATA_SET_A; 01413 } else if(*a->priority2 > *b->priority2) { 01414 return DATA_SET_B; 01415 } else if(*a->priority2 < *b->priority2) { 01416 return DATA_SET_A; 01417 } else if(*a->identity[i] > *b->identity[i]) { 01418 return DATA_SET_B; 01419 } else { 01420 return DATA_SET_A; 01421 } 01422 01423 } 01424 01426 void updateDataSet(UInteger8 code, const AnnounceMsg *msg) { 01427 01428 switch(code) { 01429 01430 case STATE_DECISION_CODE_M1: 01431 case STATE_DECISION_CODE_M2: 01432 01433 // Table 13 (.3.5). 01434 ptp.currentDS.stepsRemoved = 0; 01435 ptp.currentDS.offsetFromMaster.scaledNanoseconds = 0; 01436 ptp.currentDS.meanPathDelay.scaledNanoseconds = 0; 01437 memcpy(ptp.parentDS.parentPortIdentity. 01438 clockIdentity,ptp.defaultDS.clockIdentity, 8); 01439 ptp.parentDS.parentPortIdentity.portNumber = 0; 01440 memcpy(ptp.parentDS.grandmasterIdentity, 01441 ptp.defaultDS.clockIdentity,8); 01442 ptp.parentDS.grandmasterClockQuality = ptp.defaultDS.clockQuality; 01443 ptp.parentDS.priority1 = ptp.defaultDS.priority1; 01444 ptp.parentDS.priority2 = ptp.defaultDS.priority2; 01445 // TODO: timePropertiesDS update (9.4). 01446 /* 01447 ptp.timePropertiesDS.currentUtcOffset = 01448 ptp.timePropertiesDS.currentUtcOffsetValid = 01449 ptp.timePropertiesDS.leap59 = 01450 ptp.timePropertiesDS.leap61 = 01451 ptp.timePropertiesDS.timeTraceable = 01452 ptp.timePropertiesDS.frequencyTraceable = 01453 ptp.timePropertiesDS.ptpTimescale = 01454 ptp.timePropertiesDS.timeSource = 01455 */ 01456 break; 01457 01458 case STATE_DECISION_CODE_M3: 01459 break; 01460 01461 case STATE_DECISION_CODE_S1: 01462 01463 // Table 16. 01464 ptp.currentDS.stepsRemoved = 1 + msg->stepsRemoved; 01465 if(comparePortIdentity(&ptp.parentDS.parentPortIdentity, 01466 &msg->header.sourcePortIdentity) == TRUE) { 01467 ptp.sameMaster = TRUE; 01468 } else { 01469 ptp.sameMaster = FALSE; 01470 memcpy(ptp.parentDS.parentPortIdentity.clockIdentity, 01471 msg->header.sourcePortIdentity.clockIdentity, 8); 01472 } 01473 ptp.parentDS.parentPortIdentity.portNumber = 01474 msg->header.sourcePortIdentity.portNumber; 01475 memcpy(ptp.parentDS.grandmasterIdentity, 01476 msg->grandmasterIdentity, 8); 01477 ptp.parentDS.grandmasterClockQuality = msg->grandmasterClockQuality; 01478 ptp.parentDS.priority1 = msg->grandmasterPriority1; 01479 ptp.parentDS.priority2 = msg->grandmasterPriority2; 01480 ptp.timePropertiesDS.currentUtcOffset = msg->currentUtcOffset; 01481 ptp.timePropertiesDS.currentUtcOffsetValid = 01482 (msg->header.flagField[1] & 0x3); 01483 ptp.timePropertiesDS.leap59 = (msg->header.flagField[1] & 0x2); 01484 ptp.timePropertiesDS.leap61 = (msg->header.flagField[1] & 0x1); 01485 ptp.timePropertiesDS.timeTraceable = 01486 (msg->header.flagField[1] & 0x5); 01487 ptp.timePropertiesDS.frequencyTraceable = 01488 (msg->header.flagField[1] & 0x6); 01489 ptp.timePropertiesDS.ptpTimescale = 01490 (msg->header.flagField[1] & 0x4); 01491 ptp.timePropertiesDS.timeSource = msg->timeSource; 01492 break; 01493 01494 case STATE_DECISION_CODE_P1: 01495 break; 01496 case STATE_DECISION_CODE_P2: 01497 break; 01498 } 01499 01500 } 01501 01505 Boolean comparePortIdentity(const PortIdentity *a, const PortIdentity *b) { 01506 01507 if(memcmp(a->clockIdentity, b->clockIdentity, 8) == 0 && 01508 a->portNumber == b->portNumber) 01509 return TRUE; 01510 else 01511 return FALSE; 01512 01513 } 01514 01516 Boolean isQualified(const ForeignMasterDS *d) { 01517 01518 Integer64 timeWindow; 01519 TimeInterval timeInterval; 01520 Timestamp timestamp; 01521 getClock(×tamp); 01522 01523 if(d->msg.stepsRemoved >= 255) { 01524 return FALSE; 01525 } 01526 01527 // Find difference between current time and 2nd to last announce message. 01528 toTimeInterval(&d->announceMessageTimestamps[0], ×tamp, &timeInterval); 01529 01530 // Change back to ns. 01531 timeInterval.scaledNanoseconds = timeInterval.scaledNanoseconds >> 16; 01532 01533 // Calcuate time window in ns. 01534 timeWindow = 1000000000; 01535 timeWindow = timeWindow * 01536 (FOREIGN_MASTER_TIME_WINDOW * (1 << ptp.portDS.logAnnounceInterval)); 01537 01538 // Check if within FOREIGN_MASTER_TIME_WINDOW announce intervals. 01539 if(timeInterval.scaledNanoseconds < timeWindow) { 01540 return TRUE; 01541 } else { 01542 return FALSE; 01543 } 01544 01545 } 01546 01550 void fixTimestamps(const TimeInterval *timeInterval) { 01551 01552 UInteger8 i; 01553 TimeInterval t; 01554 t.scaledNanoseconds = -timeInterval->scaledNanoseconds; 01555 01556 for(i = 0; i < ptp.foreignMasterDSIndex; i++) { 01557 01558 addTimeIntervalToTimestamp(&t, 01559 &ptp.foreignMasterDS[i].announceMessageTimestamps[0]); 01560 addTimeIntervalToTimestamp(&t, 01561 &ptp.foreignMasterDS[i].announceMessageTimestamps[1]); 01562 01563 } 01564 01565 } 01566 01570 void printStatus() { 01571 01572 char line1[9]; 01573 char line2[9]; 01574 Integer32 o; 01575 01576 if(ptp.portDS.portState == 6) { 01577 sprintf(line1, "Master "); 01578 sprintf(line2, " "); 01579 } else if(ptp.portDS.portState == 9) { 01580 o = ptp.currentDS.offsetFromMaster.scaledNanoseconds >> 16; 01581 sprintf(line1, "Slave "); 01582 sprintf(line2, "%d", o); 01583 } else { 01584 sprintf(line1, "%d ",ptp.portDS.portState); 01585 sprintf(line2, " "); 01586 } 01587 01588 DisplayClear(); 01589 DisplayString(0, line1); 01590 DisplayString(16, line2); 01591 01592 }