XilinxProcessorIPLib/drivers/axiethernet/examples/avb/xavb_rtc_sync.c
void XAvb_CalcRtcOffset (XAvb * InstancePtr,
                         u32    PtpFrameBaseAddr) {

  u32 MasterNanosec        = 0;
  u32 MasterSeconds        = 0;
  u32 MasterEpoch          = 0;

  u32 SyncRouteDelay       = 0;
  u32 MasterNsCorrected    = 0;
  u32 MasterNsHasWrapped   = 0;

  u32 SlaveNsTimestamp     = 0;
  XAvb_RtcFormat RtcError;

  u32 BufferWordA          = 0;
  u32 BufferWordB          = 0;

  XAvb_RtcFormat SlaveRtc;


  /** Capture the Slave Time
   * ----------------------------
   * We do this immediately to get the slave time ASAP (since processing
   * time is uncertain and the RTC does not stand still). */
  XAvb_ReadRtc(InstancePtr->Config.BaseAddress, &SlaveRtc);


  /** Capture the Master Origin Timestamp (from received FollowUp Frame)
   * ---------------------------- */
  MasterNanosec = XAvb_CaptureNanoSec(InstancePtr->Config.BaseAddress,
                                      PtpFrameBaseAddr);

  /** read the Words from the PTP frame buffer containing the RTC seconds field
   */
  BufferWordA = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
                               PtpFrameBaseAddr,
                               XAVB_PTP_RX_PKT_TIMESTAMP_UPPER_OFFSET);

  BufferWordB = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
                               PtpFrameBaseAddr,
                               XAVB_PTP_RX_PKT_TIMESTAMP_MID_OFFSET);

  /** Now re-arrange the required data from the Words to obtain the required
   *  seconds field timestamp in binary format */
  MasterSeconds = (XAvb_ReorderWord(BufferWordA) << 16) |
                  (XAvb_ReorderWord(BufferWordB) >> 16);

  MasterEpoch   =  XAvb_ReorderWord(BufferWordA) >> 16;



  /** Correct the Nanoseconds
   * ----------------------------
   * NOTE: we are trying to compare the value of the slave RTC nano-
   * seconds field timestamp with the nano-seconds value of the Masters
   * RTC nanosecond field at exactly that time.
   *
   *
   * Sync Frame routing delay is equal to the value of the correction
   * field (sum of correction fields in Sync and FollowUp frames) plus
   * the link delay measurement made by this slave.
   */
   SyncRouteDelay = InstancePtr->PtpRecords.MasterCorrectionField
                    + InstancePtr->PtpRecords.LinkDelay;

  /** MasterNsCorrected time here is the calculated time that the
   * master will be at the point in time when the sync frame is received
   * (and timestamped) at the slave.  This is calculated from the
   * originTimeStamp (from the FollowUpframe), plus the Sync Frame
   * routing delay.  A direct comparison can then be made between master
   * and slave.
   */
  MasterNsCorrected = MasterNanosec + SyncRouteDelay;

  /** Check for ns wrap-around condition */
  if (MasterNsCorrected >= XAVB_ONE_SECOND) {
    MasterNsCorrected   = MasterNsCorrected - XAVB_ONE_SECOND;
    MasterNsHasWrapped  = 1;
  }


  /** Make the Master and Slave comparison and discover the difference! */
  RtcError.NanoSeconds = MasterNsCorrected
                         - InstancePtr->PtpRecords.SlaveSyncTimestamp;

  /** Check for ns wrap-around condition */
  if (RtcError.NanoSeconds >= XAVB_ONE_SECOND) {
    RtcError.NanoSeconds = RtcError.NanoSeconds + XAVB_ONE_SECOND;
  }


  /** Return these comparison figures in the form of a pointer (RTC
   * increment rate adjust function also needs to know this information) */
  InstancePtr->PtpRecords.NewSlaveTime
      = InstancePtr->PtpRecords.SlaveSyncTimestamp;

  InstancePtr->PtpRecords.NewMasterTime = MasterNsCorrected;


  /** Adjust the 8k clock logic (if necessary) */
  XAvb_Adjust8kClock(InstancePtr->Config.BaseAddress, RtcError.NanoSeconds);



  /** Correct the Seconds and Epoch
   * -----------------------------
   * NOTE: we are trying to compare the value of the slave RTC seconds
   * field at the exact time when the timestamp was taken with the
   * RTC seconds value of the Master at that time.
   *
   *
   * We need to know the value of the slaves synchronised nano-seconds
   * field at the time when the timestamp was taken (since timestamps
   * use the syntonised time).  So we add the current nanosecond field
   * offset value:
   */
  SlaveNsTimestamp = InstancePtr->PtpRecords.SlaveSyncTimestamp
                     + XAvb_ReadReg(InstancePtr->Config.BaseAddress,
                                     XAVB_RTC_NANOSEC_OFFSET);

  /** Check for ns wrap-around condition */
  if (SlaveNsTimestamp >= XAVB_ONE_SECOND) {
    SlaveNsTimestamp = SlaveNsTimestamp - XAVB_ONE_SECOND;
  }

  /** Even though we read the slave RTC value at the beginning of this
   * function, there would have been processing delay between the
   * actual reception (and timestamping) of the FollowUp frame and the
   * start of this function.  During this time, the slave RTC seconds
   * field could have wrapped around.  We need to detect this and if it
   * has done, the slave seconds field would also have incremented (so
   * it needs to be set back).
   */
  if (SlaveRtc.NanoSeconds < SlaveNsTimestamp) {
    /** slave_nanosec has wrapped since timestamp so decrement the
     * seconds field */
    if (SlaveRtc.SecondsLower == 0x00000000) {
      SlaveRtc.SecondsUpper = SlaveRtc.SecondsUpper   - 0x1;
    }
    SlaveRtc.SecondsLower = SlaveRtc.SecondsLower - 0x1;
  }


  /** If the Master nano seconds field wrapped during the Sync frame
   * routing delay, then we need to increment the seconds field.
   */
  if (MasterNsHasWrapped == 1) {
    if (MasterSeconds == 0xFFFFFFFF) {
      MasterEpoch = MasterEpoch + 0x1;
    }
    MasterSeconds = MasterSeconds + 0x1;
  }


  /** Calculate the slave RTC error: the master time minus the timestamp
   * taken by this slave for Sync Frame reception. */
  RtcError.SecondsLower =  MasterSeconds - SlaveRtc.SecondsLower;
  RtcError.SecondsUpper =  MasterEpoch   - SlaveRtc.SecondsUpper;


#ifdef DEBUG_XAVB_LEVEL2
  if (RtcError.SecondsLower != 0) {
    xil_printf("\r\nXAvb_CalcRtcOffset()");
    xil_printf("\r\n-- Seconds Field Correction");
    xil_printf("\r\nSlaveNsTimestamp : %x"  , SlaveNsTimestamp);
    xil_printf("\r\nslave_ns           : %x", SlaveRtc.NanoSeconds);
    xil_printf("\r\n--");
    xil_printf("\r\nread slave seconds : %x",
                 XAvb_ReadReg(InstancePtr->Config.BaseAddress,
                               XAVB_RTC_SEC_LOWER_VALUE_OFFSET));

    xil_printf("\r\ncalc slave secs    : %x", SlaveRtc.SecondsLower);
    xil_printf("\r\n--");
    xil_printf("\r\nmaster sec wrap  : %x"  , MasterNsHasWrapped);
    xil_printf("\r\ncalc master_secs : %x"  , MasterSeconds);
    xil_printf("\r\nrtc_sec_error : %x"     , RtcError.SecondsLower);
    xil_printf("\r\n--");
  }
#endif


  /** Write the results to the RTC Offset registers
   * --------------------------------------------- */
  XAvb_WriteRtcOffset(InstancePtr->Config.BaseAddress, &RtcError);

}
/*****************************************************************************/
/***
*
* A function to calculate the Slave Offset from the GrandMaster time
*
* @param  InstancePtr is a pointer to the XAvb instance to be worked on
* @param  PtpFrameBaseAddr is the base address of the received Announce Packet
*         in the Rx PTP Packet Buffer
*
* @return The PtpRecords data structure is updated with the calculated RTC
*         Offset value.
*
* @note   None.
*
*****************************************************************************/
A function to calculate the Slave Offset from the GrandMaster time
A function to calculate the Slave Offset from the GrandMaster time
void XAvb_CalcRtcOffset (XAvb * InstancePtr,
                         u32    PtpFrameBaseAddr) {
  u32 MasterNanosec        = 0;
  u32 MasterSeconds        = 0;
  u32 MasterEpoch          = 0;
  u32 SyncRouteDelay       = 0;
  u32 MasterNsCorrected    = 0;
  u32 MasterNsHasWrapped   = 0;
  u32 SlaveNsTimestamp     = 0;
  XAvb_RtcFormat RtcError;
  u32 BufferWordA          = 0;
  u32 BufferWordB          = 0;
  XAvb_RtcFormat SlaveRtc;
  XAvb_ReadRtc(InstancePtr->Config.BaseAddress, &SlaveRtc);
  MasterNanosec = XAvb_CaptureNanoSec(InstancePtr->Config.BaseAddress,
                                      PtpFrameBaseAddr);
  BufferWordA = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
                               PtpFrameBaseAddr,
                               XAVB_PTP_RX_PKT_TIMESTAMP_UPPER_OFFSET);
  BufferWordB = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
                               PtpFrameBaseAddr,
                               XAVB_PTP_RX_PKT_TIMESTAMP_MID_OFFSET);
  MasterSeconds = (XAvb_ReorderWord(BufferWordA) << 16) |
                  (XAvb_ReorderWord(BufferWordB) >> 16);
  MasterEpoch   =  XAvb_ReorderWord(BufferWordA) >> 16;
   SyncRouteDelay = InstancePtr->PtpRecords.MasterCorrectionField
                    + InstancePtr->PtpRecords.LinkDelay;
  MasterNsCorrected = MasterNanosec + SyncRouteDelay;
  if (MasterNsCorrected >= XAVB_ONE_SECOND) {
    MasterNsCorrected   = MasterNsCorrected - XAVB_ONE_SECOND;
    MasterNsHasWrapped  = 1;
  }
  RtcError.NanoSeconds = MasterNsCorrected
                         - InstancePtr->PtpRecords.SlaveSyncTimestamp;
  if (RtcError.NanoSeconds >= XAVB_ONE_SECOND) {
    RtcError.NanoSeconds = RtcError.NanoSeconds + XAVB_ONE_SECOND;
  }
  InstancePtr->PtpRecords.NewSlaveTime
      = InstancePtr->PtpRecords.SlaveSyncTimestamp;
  InstancePtr->PtpRecords.NewMasterTime = MasterNsCorrected;
  XAvb_Adjust8kClock(InstancePtr->Config.BaseAddress, RtcError.NanoSeconds);
  SlaveNsTimestamp = InstancePtr->PtpRecords.SlaveSyncTimestamp
                     + XAvb_ReadReg(InstancePtr->Config.BaseAddress,
                                     XAVB_RTC_NANOSEC_OFFSET);
  if (SlaveNsTimestamp >= XAVB_ONE_SECOND) {
    SlaveNsTimestamp = SlaveNsTimestamp - XAVB_ONE_SECOND;
  }
  if (SlaveRtc.NanoSeconds < SlaveNsTimestamp) {
    if (SlaveRtc.SecondsLower == 0x00000000) {
      SlaveRtc.SecondsUpper = SlaveRtc.SecondsUpper   - 0x1;
    }
    SlaveRtc.SecondsLower = SlaveRtc.SecondsLower - 0x1;
  }
  if (MasterNsHasWrapped == 1) {
    if (MasterSeconds == 0xFFFFFFFF) {
      MasterEpoch = MasterEpoch + 0x1;
    }
    MasterSeconds = MasterSeconds + 0x1;
  }
  RtcError.SecondsLower =  MasterSeconds - SlaveRtc.SecondsLower;
  RtcError.SecondsUpper =  MasterEpoch   - SlaveRtc.SecondsUpper;
#ifdef DEBUG_XAVB_LEVEL2
  if (RtcError.SecondsLower != 0) {
    xil_printf("\r\nXAvb_CalcRtcOffset()");
    xil_printf("\r\n-- Seconds Field Correction");
    xil_printf("\r\nSlaveNsTimestamp : %x"  , SlaveNsTimestamp);
    xil_printf("\r\nslave_ns           : %x", SlaveRtc.NanoSeconds);
    xil_printf("\r\n--");
    xil_printf("\r\nread slave seconds : %x",
                 XAvb_ReadReg(InstancePtr->Config.BaseAddress,
                               XAVB_RTC_SEC_LOWER_VALUE_OFFSET));
    xil_printf("\r\ncalc slave secs    : %x", SlaveRtc.SecondsLower);
    xil_printf("\r\n--");
    xil_printf("\r\nmaster sec wrap  : %x"  , MasterNsHasWrapped);
    xil_printf("\r\ncalc master_secs : %x"  , MasterSeconds);
    xil_printf("\r\nrtc_sec_error : %x"     , RtcError.SecondsLower);
    xil_printf("\r\n--");
  }
#endif
  XAvb_WriteRtcOffset(InstancePtr->Config.BaseAddress, &RtcError);
}
void XAvb_CalcRtcOffset ( XAvb * InstancePtr , u32 PtpFrameBaseAddr ) { u32 MasterNanosec = 0 ; u32 MasterSeconds = 0 ; u32 MasterEpoch = 0 ; u32 SyncRouteDelay = 0 ; u32 MasterNsCorrected = 0 ; u32 MasterNsHasWrapped = 0 ; u32 SlaveNsTimestamp = 0 ; XAvb_RtcFormat RtcError ; u32 BufferWordA = 0 ; u32 BufferWordB = 0 ; XAvb_RtcFormat SlaveRtc ; XAvb_ReadRtc ( InstancePtr -> Config . BaseAddress , & SlaveRtc ) ; MasterNanosec = XAvb_CaptureNanoSec ( InstancePtr -> Config . BaseAddress , PtpFrameBaseAddr ) ; BufferWordA = XAvb_ReadPtpBuffer ( InstancePtr -> Config . BaseAddress , PtpFrameBaseAddr , XAVB_PTP_RX_PKT_TIMESTAMP_UPPER_OFFSET ) ; BufferWordB = XAvb_ReadPtpBuffer ( InstancePtr -> Config . BaseAddress , PtpFrameBaseAddr , XAVB_PTP_RX_PKT_TIMESTAMP_MID_OFFSET ) ; MasterSeconds = ( XAvb_ReorderWord ( BufferWordA ) << 16 ) | ( XAvb_ReorderWord ( BufferWordB ) >> 16 ) ; MasterEpoch = XAvb_ReorderWord ( BufferWordA ) >> 16 ; SyncRouteDelay = InstancePtr -> PtpRecords . MasterCorrectionField + InstancePtr -> PtpRecords . LinkDelay ; MasterNsCorrected = MasterNanosec + SyncRouteDelay ; if ( MasterNsCorrected >= XAVB_ONE_SECOND ) { MasterNsCorrected = MasterNsCorrected - XAVB_ONE_SECOND ; MasterNsHasWrapped = 1 ; } RtcError . NanoSeconds = MasterNsCorrected - InstancePtr -> PtpRecords . SlaveSyncTimestamp ; if ( RtcError . NanoSeconds >= XAVB_ONE_SECOND ) { RtcError . NanoSeconds = RtcError . NanoSeconds + XAVB_ONE_SECOND ; } InstancePtr -> PtpRecords . NewSlaveTime = InstancePtr -> PtpRecords . SlaveSyncTimestamp ; InstancePtr -> PtpRecords . NewMasterTime = MasterNsCorrected ; XAvb_Adjust8kClock ( InstancePtr -> Config . BaseAddress , RtcError . NanoSeconds ) ; SlaveNsTimestamp = InstancePtr -> PtpRecords . SlaveSyncTimestamp + XAvb_ReadReg ( InstancePtr -> Config . BaseAddress , XAVB_RTC_NANOSEC_OFFSET ) ; if ( SlaveNsTimestamp >= XAVB_ONE_SECOND ) { SlaveNsTimestamp = SlaveNsTimestamp - XAVB_ONE_SECOND ; } if ( SlaveRtc . NanoSeconds < SlaveNsTimestamp ) { if ( SlaveRtc . SecondsLower == 0x00000000 ) { SlaveRtc . SecondsUpper = SlaveRtc . SecondsUpper - 0x1 ; } SlaveRtc . SecondsLower = SlaveRtc . SecondsLower - 0x1 ; } if ( MasterNsHasWrapped == 1 ) { if ( MasterSeconds == 0xFFFFFFFF ) { MasterEpoch = MasterEpoch + 0x1 ; } MasterSeconds = MasterSeconds + 0x1 ; } RtcError . SecondsLower = MasterSeconds - SlaveRtc . SecondsLower ; RtcError . SecondsUpper = MasterEpoch - SlaveRtc . SecondsUpper ; #ifdef DEBUG_XAVB_LEVEL2 if ( RtcError . SecondsLower != 0 ) { xil_printf ( " \r \n " ) ; xil_printf ( " \r \n " ) ; xil_printf ( " \r \n " , SlaveNsTimestamp ) ; xil_printf ( " \r \n " , SlaveRtc . NanoSeconds ) ; xil_printf ( " \r \n " ) ; xil_printf ( " \r \n " , XAvb_ReadReg ( InstancePtr -> Config . BaseAddress , XAVB_RTC_SEC_LOWER_VALUE_OFFSET ) ) ; xil_printf ( " \r \n " , SlaveRtc . SecondsLower ) ; xil_printf ( " \r \n " ) ; xil_printf ( " \r \n " , MasterNsHasWrapped ) ; xil_printf ( " \r \n " , MasterSeconds ) ; xil_printf ( " \r \n " , RtcError . SecondsLower ) ; xil_printf ( " \r \n " ) ; } #endif XAvb_WriteRtcOffset ( InstancePtr -> Config . BaseAddress , & RtcError ) ; }
A function to calculate the Slave Offset from the GrandMaster time
A function to calculate the Slave Offset from the GrandMaster time
/** Capture the Slave Time
   * ----------------------------
   * We do this immediately to get the slave time ASAP (since processing
   * time is uncertain and the RTC does not stand still). */ /** Capture the Master Origin Timestamp (from received FollowUp Frame)
   * ---------------------------- */ /** read the Words from the PTP frame buffer containing the RTC seconds field
   */ /** Now re-arrange the required data from the Words to obtain the required
   *  seconds field timestamp in binary format */ /** Correct the Nanoseconds
   * ----------------------------
   * NOTE: we are trying to compare the value of the slave RTC nano-
   * seconds field timestamp with the nano-seconds value of the Masters
   * RTC nanosecond field at exactly that time.
   *
   *
   * Sync Frame routing delay is equal to the value of the correction
   * field (sum of correction fields in Sync and FollowUp frames) plus
   * the link delay measurement made by this slave.
   */ /** MasterNsCorrected time here is the calculated time that the
   * master will be at the point in time when the sync frame is received
   * (and timestamped) at the slave.  This is calculated from the
   * originTimeStamp (from the FollowUpframe), plus the Sync Frame
   * routing delay.  A direct comparison can then be made between master
   * and slave.
   */ /** Check for ns wrap-around condition */ /** Make the Master and Slave comparison and discover the difference! */ /** Check for ns wrap-around condition */ /** Return these comparison figures in the form of a pointer (RTC
   * increment rate adjust function also needs to know this information) */ /** Adjust the 8k clock logic (if necessary) */ /** Correct the Seconds and Epoch
   * -----------------------------
   * NOTE: we are trying to compare the value of the slave RTC seconds
   * field at the exact time when the timestamp was taken with the
   * RTC seconds value of the Master at that time.
   *
   *
   * We need to know the value of the slaves synchronised nano-seconds
   * field at the time when the timestamp was taken (since timestamps
   * use the syntonised time).  So we add the current nanosecond field
   * offset value:
   */ /** Check for ns wrap-around condition */ /** Even though we read the slave RTC value at the beginning of this
   * function, there would have been processing delay between the
   * actual reception (and timestamping) of the FollowUp frame and the
   * start of this function.  During this time, the slave RTC seconds
   * field could have wrapped around.  We need to detect this and if it
   * has done, the slave seconds field would also have incremented (so
   * it needs to be set back).
   */ /** slave_nanosec has wrapped since timestamp so decrement the
     * seconds field */ /** If the Master nano seconds field wrapped during the Sync frame
   * routing delay, then we need to increment the seconds field.
   */ /** Calculate the slave RTC error: the master time minus the timestamp
   * taken by this slave for Sync Frame reception. */ /** Write the results to the RTC Offset registers
   * --------------------------------------------- */