XilinxProcessorIPLib/drivers/axiethernet/examples/avb/xavb_rtc_sync.c
void XAvb_UpdateRtcIncrement(XAvb * InstancePtr) {

  u32 LoopCount           = 31;
  u8  SlaveIsFast         = 0;
  u32 SlaveTimeDuration   = 0;
  u32 MasterTimeDuration  = 0;
  u32 SlaveError          = 0;
  u32 ScaledError         = 0;
  u32 NormalisedError     = 0;
  u32 IncrementAdjust     = 0;
  u32 OldIncrement        = 0;
  u32 NewIncrement        = 0;


  /** Sanity Check that Sync Frames were n apart.  This safeguards the
   * calculation against the ethernet cable being pulled out and then
   * replaced, etc. */
  if ( ((InstancePtr->SequenceIdRecords.OldSyncSequenceId +
         XAVB_NUM_SYNC_FU_PAIR_CALC_RTC_INCREMENT) & 0xFFFF) ==
         InstancePtr->SequenceIdRecords.NewSyncSequenceId ) {

#ifdef DEBUG_XAVB_LEVEL2
    xil_printf("\r\nXAvb_UpdateRtcIncrement(): Debug...(a)");
    xil_printf("\r\nNewMasterTime : %x, %d" , InstancePtr->PtpRecords.NewMasterTime, InstancePtr->PtpRecords.NewMasterTime);
    xil_printf("\r\nOldMasterTime : %x, %d" , InstancePtr->PtpRecords.OldMasterTime, InstancePtr->PtpRecords.OldMasterTime);
    xil_printf("\r\nNewSlaveTime : %x, %d"  , InstancePtr->PtpRecords.NewSlaveTime,  InstancePtr->PtpRecords.NewSlaveTime);
    xil_printf("\r\nOldSlaveTime : %x, %d\r\n"  , InstancePtr->PtpRecords.OldSlaveTime,  InstancePtr->PtpRecords.OldSlaveTime);
#endif

    /** Measure the time duration, as measured by the RTC master of the
     * M sync delay measurement period. */
    MasterTimeDuration = (InstancePtr->PtpRecords.NewMasterTime
                         - InstancePtr->PtpRecords.OldMasterTime);

    if (MasterTimeDuration >= XAVB_ONE_SECOND) {
      MasterTimeDuration = MasterTimeDuration + XAVB_ONE_SECOND;
    }

    /** Measure the time duration, as measured by the RTC slave of the
     * M sync delay measurement period. */
    SlaveTimeDuration  = (InstancePtr->PtpRecords.NewSlaveTime
                         - InstancePtr->PtpRecords.OldSlaveTime);

    if (SlaveTimeDuration >= XAVB_ONE_SECOND) {
      SlaveTimeDuration = SlaveTimeDuration + XAVB_ONE_SECOND;
    }

    /** Therefore calculate the slave error (in ns) */
    SlaveError = MasterTimeDuration - SlaveTimeDuration;

    /** If the slave error is zero, skip the remainder of function.
     * (Note : a zero error would otherwise get stuck in the while loop
     *         further down this function). */
    if (SlaveError != 0) {


       /** Analyse msb of error signal to see which clock is running fastest */
       if (SlaveError & 0x80000000) {
         SlaveIsFast = 1;
         SlaveError   = SlaveTimeDuration - MasterTimeDuration;
       } else {
         SlaveIsFast = 0;
       }

       /** This check is in addition to the checks described in IEEE802.1as.
        *  If the SlaveError is unexpectedly large, then set asCapable to 0.
        */
       if (SlaveError < XAVB_CLOCK_LOCK_THRESHOLD) {
         XAvb_ChangePTPLockStatus(InstancePtr, 1);
       } else {
         XAvb_ChangePTPLockStatus(InstancePtr, 0);
       }

       /** SlaveError signal is 32-bits (ns).  This can indicate > 4 sec of
        * error: this is too large for 100 ms measurement period.  So we
        * expect upper bits to be zero.
        *
        * This function will shift the 1st none zero bit of SlaveError up
        * to bit 31, so that forthcoming calculation uses maximum accuracy.
        *
        * This shift is equivalent to a multiply (of the error signal). A
        * shift the opposite way (equivalent to a divide) will follow at
        * end of full calculation. */

       while ( !(SlaveError & (0x1 << LoopCount)) ) {
         LoopCount = LoopCount - 1;
       }
       LoopCount = 31 - LoopCount;
       ScaledError  = (SlaveError << LoopCount);


       /** Calculate the relative error: can be thought of as a scaled ratio
        * of error per time unit */
       NormalisedError = ScaledError / MasterTimeDuration;


       /** Obtain the current increment value */
       OldIncrement = (XAvb_ReadReg(InstancePtr->Config.BaseAddress,
                                     XAVB_RTC_INCREMENT_OFFSET)
                       & XAVB_RTC_INCREMENT_VALUE_MASK);


       /** Calculate the increment adjustment: multiply NormalisedError by
        * the increment time unit.  Then shift back the other way to
        * correct the calculation (restore to ns). */
       IncrementAdjust = (NormalisedError * OldIncrement) >> LoopCount;


       /** Now calculate the new increment value */
       if (SlaveIsFast) {
         NewIncrement = OldIncrement - IncrementAdjust;
       } else {
         NewIncrement = OldIncrement + IncrementAdjust;
       }

       /** Add some rails so that recovery is possible after a
        *  string of bad pDelay values.  The RTC should be able to lock
        *  to within 100ppm of the slowest allowable clock (25 MHz).
        *  This equates to +/-4ps.  Let's arbitrarily set the rails to
        *  400ppm (+/-16ps) just in case someone decides to use a
        *  particularly bad oscillator.  The lowest 20 bits of
        *  NewIncrement are fractions of a nanosecond, which equates
        *  to +/- 0x04189
        */
       if( NewIncrement > (XAVB_RTC_INCREMENT_NOMINAL_RATE + XAVB_RTC_400PPM_OFFSET) ) {
         xil_printf("\r\nRTC Exceeded 400ppm offset: Railing to 400ppm\r\n");
         NewIncrement = XAVB_RTC_INCREMENT_NOMINAL_RATE + XAVB_RTC_400PPM_OFFSET;
       }
       if( NewIncrement < (XAVB_RTC_INCREMENT_NOMINAL_RATE - XAVB_RTC_400PPM_OFFSET) ) {
         xil_printf("\r\nRTC Exceeded 400ppm offset: Railing to 400ppm\r\n");
         NewIncrement = XAVB_RTC_INCREMENT_NOMINAL_RATE - XAVB_RTC_400PPM_OFFSET;
       }

       /** And write the new increment value! */
       XAvb_WriteReg(InstancePtr->Config.BaseAddress,
                      XAVB_RTC_INCREMENT_OFFSET,
                      NewIncrement);

#ifdef DEBUG_XAVB_LEVEL2
    xil_printf("\r\nXAvb_UpdateRtcIncrement(): Debug...");
    xil_printf("\r\nM Time : %x" , MasterTimeDuration);
    xil_printf("\r\nS Time : %x" , SlaveTimeDuration);
    xil_printf("\r\nErr : %x %x" , SlaveIsFast, SlaveError);
    xil_printf("\r\nScaled : %x" , ScaledError);
    xil_printf("\r\nNorm   : %x" , NormalisedError);
    xil_printf("\r\nAdjust : %x" , IncrementAdjust);
    xil_printf("\r\nNew Inc: %x" , NewIncrement);
#endif

    }
  } else {

    xil_printf("\r\nXAvb_UpdateRtcIncrement()");
    xil_printf("\r\nERROR: Syncs not %d apart - %d\r\n",
               XAVB_NUM_SYNC_FU_PAIR_CALC_RTC_INCREMENT,
               InstancePtr->SequenceIdRecords.NewSyncSequenceId -
               InstancePtr->SequenceIdRecords.OldSyncSequenceId);
  }

  if (SlaveError > 0x2700) {
    xil_printf("\r\nXAvb_UpdateRtcIncrement(): Large Error over 100ms");
    xil_printf("\r\nM Time : %x" , MasterTimeDuration);
    xil_printf("\r\nS Time : %x" , SlaveTimeDuration);
    xil_printf("\r\nErr : %x %x" , SlaveIsFast, SlaveError);
    xil_printf("\r\nScaled : %x" , ScaledError);
    xil_printf("\r\nNorm   : %x" , NormalisedError);
    xil_printf("\r\nAdjust : %x" , IncrementAdjust);
    xil_printf("\r\nNew Inc: %x" , NewIncrement);
}

}
/*****************************************************************************/
/***
*
* A function to calculate the RTC increment value based on the Slave Error
*
* @return None. But the devices RTC Increment Value Control Register is updated
*
* @note   None.
*
*****************************************************************************/
A function to calculate the RTC increment value based on the Slave Error
A function to calculate the RTC increment value based on the Slave Error
void XAvb_UpdateRtcIncrement(XAvb * InstancePtr) {
  u32 LoopCount           = 31;
  u8  SlaveIsFast         = 0;
  u32 SlaveTimeDuration   = 0;
  u32 MasterTimeDuration  = 0;
  u32 SlaveError          = 0;
  u32 ScaledError         = 0;
  u32 NormalisedError     = 0;
  u32 IncrementAdjust     = 0;
  u32 OldIncrement        = 0;
  u32 NewIncrement        = 0;
  if ( ((InstancePtr->SequenceIdRecords.OldSyncSequenceId +
         XAVB_NUM_SYNC_FU_PAIR_CALC_RTC_INCREMENT) & 0xFFFF) ==
         InstancePtr->SequenceIdRecords.NewSyncSequenceId ) {
#ifdef DEBUG_XAVB_LEVEL2
    xil_printf("\r\nXAvb_UpdateRtcIncrement(): Debug...(a)");
    xil_printf("\r\nNewMasterTime : %x, %d" , InstancePtr->PtpRecords.NewMasterTime, InstancePtr->PtpRecords.NewMasterTime);
    xil_printf("\r\nOldMasterTime : %x, %d" , InstancePtr->PtpRecords.OldMasterTime, InstancePtr->PtpRecords.OldMasterTime);
    xil_printf("\r\nNewSlaveTime : %x, %d"  , InstancePtr->PtpRecords.NewSlaveTime,  InstancePtr->PtpRecords.NewSlaveTime);
    xil_printf("\r\nOldSlaveTime : %x, %d\r\n"  , InstancePtr->PtpRecords.OldSlaveTime,  InstancePtr->PtpRecords.OldSlaveTime);
#endif
    MasterTimeDuration = (InstancePtr->PtpRecords.NewMasterTime
                         - InstancePtr->PtpRecords.OldMasterTime);
    if (MasterTimeDuration >= XAVB_ONE_SECOND) {
      MasterTimeDuration = MasterTimeDuration + XAVB_ONE_SECOND;
    }
    SlaveTimeDuration  = (InstancePtr->PtpRecords.NewSlaveTime
                         - InstancePtr->PtpRecords.OldSlaveTime);
    if (SlaveTimeDuration >= XAVB_ONE_SECOND) {
      SlaveTimeDuration = SlaveTimeDuration + XAVB_ONE_SECOND;
    }
    SlaveError = MasterTimeDuration - SlaveTimeDuration;
    if (SlaveError != 0) {
       if (SlaveError & 0x80000000) {
         SlaveIsFast = 1;
         SlaveError   = SlaveTimeDuration - MasterTimeDuration;
       } else {
         SlaveIsFast = 0;
       }
       if (SlaveError < XAVB_CLOCK_LOCK_THRESHOLD) {
         XAvb_ChangePTPLockStatus(InstancePtr, 1);
       } else {
         XAvb_ChangePTPLockStatus(InstancePtr, 0);
       }
       while ( !(SlaveError & (0x1 << LoopCount)) ) {
         LoopCount = LoopCount - 1;
       }
       LoopCount = 31 - LoopCount;
       ScaledError  = (SlaveError << LoopCount);
       NormalisedError = ScaledError / MasterTimeDuration;
       OldIncrement = (XAvb_ReadReg(InstancePtr->Config.BaseAddress,
                                     XAVB_RTC_INCREMENT_OFFSET)
                       & XAVB_RTC_INCREMENT_VALUE_MASK);
       IncrementAdjust = (NormalisedError * OldIncrement) >> LoopCount;
       if (SlaveIsFast) {
         NewIncrement = OldIncrement - IncrementAdjust;
       } else {
         NewIncrement = OldIncrement + IncrementAdjust;
       }
       if( NewIncrement > (XAVB_RTC_INCREMENT_NOMINAL_RATE + XAVB_RTC_400PPM_OFFSET) ) {
         xil_printf("\r\nRTC Exceeded 400ppm offset: Railing to 400ppm\r\n");
         NewIncrement = XAVB_RTC_INCREMENT_NOMINAL_RATE + XAVB_RTC_400PPM_OFFSET;
       }
       if( NewIncrement < (XAVB_RTC_INCREMENT_NOMINAL_RATE - XAVB_RTC_400PPM_OFFSET) ) {
         xil_printf("\r\nRTC Exceeded 400ppm offset: Railing to 400ppm\r\n");
         NewIncrement = XAVB_RTC_INCREMENT_NOMINAL_RATE - XAVB_RTC_400PPM_OFFSET;
       }
       XAvb_WriteReg(InstancePtr->Config.BaseAddress,
                      XAVB_RTC_INCREMENT_OFFSET,
                      NewIncrement);
#ifdef DEBUG_XAVB_LEVEL2
    xil_printf("\r\nXAvb_UpdateRtcIncrement(): Debug...");
    xil_printf("\r\nM Time : %x" , MasterTimeDuration);
    xil_printf("\r\nS Time : %x" , SlaveTimeDuration);
    xil_printf("\r\nErr : %x %x" , SlaveIsFast, SlaveError);
    xil_printf("\r\nScaled : %x" , ScaledError);
    xil_printf("\r\nNorm   : %x" , NormalisedError);
    xil_printf("\r\nAdjust : %x" , IncrementAdjust);
    xil_printf("\r\nNew Inc: %x" , NewIncrement);
#endif
    }
  } else {
    xil_printf("\r\nXAvb_UpdateRtcIncrement()");
    xil_printf("\r\nERROR: Syncs not %d apart - %d\r\n",
               XAVB_NUM_SYNC_FU_PAIR_CALC_RTC_INCREMENT,
               InstancePtr->SequenceIdRecords.NewSyncSequenceId -
               InstancePtr->SequenceIdRecords.OldSyncSequenceId);
  }
  if (SlaveError > 0x2700) {
    xil_printf("\r\nXAvb_UpdateRtcIncrement(): Large Error over 100ms");
    xil_printf("\r\nM Time : %x" , MasterTimeDuration);
    xil_printf("\r\nS Time : %x" , SlaveTimeDuration);
    xil_printf("\r\nErr : %x %x" , SlaveIsFast, SlaveError);
    xil_printf("\r\nScaled : %x" , ScaledError);
    xil_printf("\r\nNorm   : %x" , NormalisedError);
    xil_printf("\r\nAdjust : %x" , IncrementAdjust);
    xil_printf("\r\nNew Inc: %x" , NewIncrement);
}
}
void XAvb_UpdateRtcIncrement ( XAvb * InstancePtr ) { u32 LoopCount = 31 ; u8 SlaveIsFast = 0 ; u32 SlaveTimeDuration = 0 ; u32 MasterTimeDuration = 0 ; u32 SlaveError = 0 ; u32 ScaledError = 0 ; u32 NormalisedError = 0 ; u32 IncrementAdjust = 0 ; u32 OldIncrement = 0 ; u32 NewIncrement = 0 ; if ( ( ( InstancePtr -> SequenceIdRecords . OldSyncSequenceId + XAVB_NUM_SYNC_FU_PAIR_CALC_RTC_INCREMENT ) & 0xFFFF ) == InstancePtr -> SequenceIdRecords . NewSyncSequenceId ) { #ifdef DEBUG_XAVB_LEVEL2 xil_printf ( " \r \n " ) ; xil_printf ( " \r \n " , InstancePtr -> PtpRecords . NewMasterTime , InstancePtr -> PtpRecords . NewMasterTime ) ; xil_printf ( " \r \n " , InstancePtr -> PtpRecords . OldMasterTime , InstancePtr -> PtpRecords . OldMasterTime ) ; xil_printf ( " \r \n " , InstancePtr -> PtpRecords . NewSlaveTime , InstancePtr -> PtpRecords . NewSlaveTime ) ; xil_printf ( " \r \n \r \n " , InstancePtr -> PtpRecords . OldSlaveTime , InstancePtr -> PtpRecords . OldSlaveTime ) ; #endif MasterTimeDuration = ( InstancePtr -> PtpRecords . NewMasterTime - InstancePtr -> PtpRecords . OldMasterTime ) ; if ( MasterTimeDuration >= XAVB_ONE_SECOND ) { MasterTimeDuration = MasterTimeDuration + XAVB_ONE_SECOND ; } SlaveTimeDuration = ( InstancePtr -> PtpRecords . NewSlaveTime - InstancePtr -> PtpRecords . OldSlaveTime ) ; if ( SlaveTimeDuration >= XAVB_ONE_SECOND ) { SlaveTimeDuration = SlaveTimeDuration + XAVB_ONE_SECOND ; } SlaveError = MasterTimeDuration - SlaveTimeDuration ; if ( SlaveError != 0 ) { if ( SlaveError & 0x80000000 ) { SlaveIsFast = 1 ; SlaveError = SlaveTimeDuration - MasterTimeDuration ; } else { SlaveIsFast = 0 ; } if ( SlaveError < XAVB_CLOCK_LOCK_THRESHOLD ) { XAvb_ChangePTPLockStatus ( InstancePtr , 1 ) ; } else { XAvb_ChangePTPLockStatus ( InstancePtr , 0 ) ; } while ( ! ( SlaveError & ( 0x1 << LoopCount ) ) ) { LoopCount = LoopCount - 1 ; } LoopCount = 31 - LoopCount ; ScaledError = ( SlaveError << LoopCount ) ; NormalisedError = ScaledError / MasterTimeDuration ; OldIncrement = ( XAvb_ReadReg ( InstancePtr -> Config . BaseAddress , XAVB_RTC_INCREMENT_OFFSET ) & XAVB_RTC_INCREMENT_VALUE_MASK ) ; IncrementAdjust = ( NormalisedError * OldIncrement ) >> LoopCount ; if ( SlaveIsFast ) { NewIncrement = OldIncrement - IncrementAdjust ; } else { NewIncrement = OldIncrement + IncrementAdjust ; } if ( NewIncrement > ( XAVB_RTC_INCREMENT_NOMINAL_RATE + XAVB_RTC_400PPM_OFFSET ) ) { xil_printf ( " \r \n \r \n " ) ; NewIncrement = XAVB_RTC_INCREMENT_NOMINAL_RATE + XAVB_RTC_400PPM_OFFSET ; } if ( NewIncrement < ( XAVB_RTC_INCREMENT_NOMINAL_RATE - XAVB_RTC_400PPM_OFFSET ) ) { xil_printf ( " \r \n \r \n " ) ; NewIncrement = XAVB_RTC_INCREMENT_NOMINAL_RATE - XAVB_RTC_400PPM_OFFSET ; } XAvb_WriteReg ( InstancePtr -> Config . BaseAddress , XAVB_RTC_INCREMENT_OFFSET , NewIncrement ) ; #ifdef DEBUG_XAVB_LEVEL2 xil_printf ( " \r \n " ) ; xil_printf ( " \r \n " , MasterTimeDuration ) ; xil_printf ( " \r \n " , SlaveTimeDuration ) ; xil_printf ( " \r \n " , SlaveIsFast , SlaveError ) ; xil_printf ( " \r \n " , ScaledError ) ; xil_printf ( " \r \n " , NormalisedError ) ; xil_printf ( " \r \n " , IncrementAdjust ) ; xil_printf ( " \r \n " , NewIncrement ) ; #endif } } else { xil_printf ( " \r \n " ) ; xil_printf ( " \r \n \r \n " , XAVB_NUM_SYNC_FU_PAIR_CALC_RTC_INCREMENT , InstancePtr -> SequenceIdRecords . NewSyncSequenceId - InstancePtr -> SequenceIdRecords . OldSyncSequenceId ) ; } if ( SlaveError > 0x2700 ) { xil_printf ( " \r \n " ) ; xil_printf ( " \r \n " , MasterTimeDuration ) ; xil_printf ( " \r \n " , SlaveTimeDuration ) ; xil_printf ( " \r \n " , SlaveIsFast , SlaveError ) ; xil_printf ( " \r \n " , ScaledError ) ; xil_printf ( " \r \n " , NormalisedError ) ; xil_printf ( " \r \n " , IncrementAdjust ) ; xil_printf ( " \r \n " , NewIncrement ) ; } }
A function to calculate the RTC increment value based on the Slave Error
A function to calculate the RTC increment value based on the Slave Error
/** Sanity Check that Sync Frames were n apart.  This safeguards the
   * calculation against the ethernet cable being pulled out and then
   * replaced, etc. */ /** Measure the time duration, as measured by the RTC master of the
     * M sync delay measurement period. */ /** Measure the time duration, as measured by the RTC slave of the
     * M sync delay measurement period. */ /** Therefore calculate the slave error (in ns) */ /** If the slave error is zero, skip the remainder of function.
     * (Note : a zero error would otherwise get stuck in the while loop
     *         further down this function). */ /** Analyse msb of error signal to see which clock is running fastest */ /** This check is in addition to the checks described in IEEE802.1as.
        *  If the SlaveError is unexpectedly large, then set asCapable to 0.
        */ /** SlaveError signal is 32-bits (ns).  This can indicate > 4 sec of
        * error: this is too large for 100 ms measurement period.  So we
        * expect upper bits to be zero.
        *
        * This function will shift the 1st none zero bit of SlaveError up
        * to bit 31, so that forthcoming calculation uses maximum accuracy.
        *
        * This shift is equivalent to a multiply (of the error signal). A
        * shift the opposite way (equivalent to a divide) will follow at
        * end of full calculation. */ /** Calculate the relative error: can be thought of as a scaled ratio
        * of error per time unit */ /** Obtain the current increment value */ /** Calculate the increment adjustment: multiply NormalisedError by
        * the increment time unit.  Then shift back the other way to
        * correct the calculation (restore to ns). */ /** Now calculate the new increment value */ /** Add some rails so that recovery is possible after a
        *  string of bad pDelay values.  The RTC should be able to lock
        *  to within 100ppm of the slowest allowable clock (25 MHz).
        *  This equates to +/-4ps.  Let's arbitrarily set the rails to
        *  400ppm (+/-16ps) just in case someone decides to use a
        *  particularly bad oscillator.  The lowest 20 bits of
        *  NewIncrement are fractions of a nanosecond, which equates
        *  to +/- 0x04189
        */ /** And write the new increment value! */