pt1588-SH7216 1
IEEE1588v2 Implementation for Renesas SH7216 Demo

C:/Users/mzimmer/IEEE1588/pt1588/branches/pt1588-SH7216/pt1588/ptp-protocol.c

Go to the documentation of this file.
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(&timestamp);
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], &timestamp, &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 }
 All Data Structures Files Functions Variables Typedefs Enumerator Defines