/******************************************************************************
*
* Copyright (C) 2008 - 2014 Xilinx, Inc. All rights reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* Use of the Software is limited solely to applications:
* (a) running on a Xilinx device, or
* (b) that interact with a Xilinx device through a bus or interconnect.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* XILINX CONSORTIUM BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
* OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*
* Except as contained in this notice, the name of the Xilinx shall not be used
* in advertising or otherwise to promote the sale, use or other dealings in
* this Software without prior written authorization from Xilinx.
*
******************************************************************************/
/*****************************************************************************/
/**
*
* @file xavb_ptp_packets.c
*
* The XAvb driver. Functions in this file all contain functions which decode the
* received Precise Timing Protocol (PTP) frames, or to format and transmit PTP
* frames.
*
* <pre>
* MODIFICATION HISTORY:
*
* Ver Who Date Changes
* ----- ---- -------- -----------------------------------------------
* 1.00a mbr 09/19/08 First release
* 1.01a mbr 06/24/09 PTP frame format updates for IEEE802.1 AS draft 5-0
* 2_02a mbr 09/16/09 Updates for programmable PTP timers
* 2_04a kag 07/23/10 PTP frame format updates for IEEE802.1 AS draft 6-7
* 3_01a kag 08/29/11 Added new APIs to update the RX Filter Control Reg.
* Fix for CR:572539. Updated bit map for Rx Filter
* control reg.
*
* </pre>
*
******************************************************************************/
/***************************** Include Files *********************************/
#include "xil_types.h"
#include "xavb_hw.h"
#include "xavb.h"
#include "stdlib.h"
/************************** Constant Definitions *****************************/
/**************************** Type Definitions *******************************/
/***************** Macros (Inline Functions) Definitions *********************/
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
/*****************************************************************************/
/****************************************************************************/
/**
*
* A function to compare two ClockIdentity values.
*
* @param BaseAddress is the base address of the device
* @param Identity1 is the first ClockIdentity to be compared
* @param Identity2 is the second ClockIdentity to be compared
*
* @return 1 if the two values are equal, 0 if not equal
*
* @note
*
*****************************************************************************/
u32 XAvb_CompareClockIdentity(u32 BaseAddress,
XAvb_ClockIdentity Identity1,
XAvb_ClockIdentity Identity2) {
if( Identity1.ClockIdentityUpper != Identity2.ClockIdentityUpper ) {
/*xil_printf("ID1Upper (0x%08x) != ID2Upper (0x%08x)\r\n",
Identity1.ClockIdentityUpper,
Identity2.ClockIdentityUpper);*/
return 0;
}
if( Identity1.ClockIdentityLower != Identity2.ClockIdentityLower ) {
/*xil_printf("ID1Lower (0x%08x) != ID2Lower (0x%08x)\r\n",
Identity1.ClockIdentityLower,
Identity2.ClockIdentityLower);*/
return 0;
}
/** values are equal */
return 1;
}
/****************************************************************************/
/**
*
* A function to compare two PortIdentity values.
*
* @param BaseAddress is the base address of the device
* @param Identity1 is the first sourcePortIdentity to be compared
* @param Identity2 is the second sourcePortIdentity to be compared
*
* @return 1 if the two values are equal, 0 if not equal
*
* @note None.
*
*****************************************************************************/
u32 XAvb_ComparePortIdentity(u32 BaseAddress,
XAvb_PortIdentity Identity1,
XAvb_PortIdentity Identity2) {
if( Identity1.ClockIdentityUpper != Identity2.ClockIdentityUpper ) {
/*xil_printf("ID1Upper (0x%08x) != ID2Upper (0x%08x)\r\n",
Identity1.ClockIdentityUpper,
Identity2.ClockIdentityUpper);*/
return 0;
}
if( Identity1.ClockIdentityLower != Identity2.ClockIdentityLower ) {
/*xil_printf("ID1Lower (0x%08x) != ID2Lower (0x%08x)\r\n",
Identity1.ClockIdentityLower,
Identity2.ClockIdentityLower);*/
return 0;
}
if( Identity1.PortNumber != Identity2.PortNumber ) {
/*xil_printf("ID1Port (0x%08x) != ID2Port (0x%08x)\r\n",
Identity1.PortNumber,
Identity2.PortNumber);*/
return 0;
}
/** values are equal */
return 1;
}
/****************************************************************************/
/**
*
* A function to extract portIdentity information from a received PTP frame.
* This can be any portIdentity field (header portIdentity, requestingPortIdentity,
* etc.)
*
* @param BaseAddress is the base address of the device
* @param PtpFrameBaseAddr is the base address of the received Announce Packet
* in the Rx PTP Packet Buffer
* @param PortIdOffset is the packet offset of the first byte of the portIdentity
* field to be parsed
* @param portID is the XAvb_PortIdentity struct that the data will be written to.
*
* @return None, but portID will be updated with the portIdentity information
*
* @note None.
*
*****************************************************************************/
void XAvb_GetPortIdentity(u32 BaseAddress, u32 PtpFrameBaseAddr,
u32 PortIdOffset, XAvb_PortIdentity *portID) {
u32 ReadWord;
ReadWord = XAvb_ReorderWord(XAvb_ReadPtpBuffer(BaseAddress, PtpFrameBaseAddr, PortIdOffset));
portID->ClockIdentityUpper = (ReadWord << 16);
ReadWord = XAvb_ReorderWord(XAvb_ReadPtpBuffer(BaseAddress, PtpFrameBaseAddr, PortIdOffset + 4));
portID->ClockIdentityUpper |= (ReadWord >> 16);
portID->ClockIdentityLower = (ReadWord << 16);
ReadWord = XAvb_ReorderWord(XAvb_ReadPtpBuffer(BaseAddress, PtpFrameBaseAddr, PortIdOffset + 8));
portID->ClockIdentityLower |= (ReadWord >> 16);
portID->PortNumber = ReadWord;
}
/****************************************************************************/
/**
*
* A function to write common data (eg the Source Address) to all PTP frames
* stored in the Tx PTP Packet buffer
*
* @param BaseAddress is the base address of the device
* @param PtpFieldAddress is the offset address of the relevant field in PTP
* frames.
* @param Data is the common data to be written to all Tx PTP frame templates
* @param DataBitEnable allows only selected bits of the 32-bit Data word to
* be modified.
* @param BufferEnable allows the selected buffer to be seleced: there are 8
* PTP buffers - these are encoded as one-hot. For example, 0x3F will
* write the selected data to the first 6 buffers only.
*
* @return None. But the Tx PTP Packet Buffer is written to as requested
*
* @note None.
*
*****************************************************************************/
void XAvb_WriteToMultipleTxPtpFrames(u32 BaseAddress,
u32 PtpFieldAddress,
u32 Data,
u32 DataBitEnable,
u8 BufferEnable) {
u32 PtpBufferPointer = 0;
u32 LocalData = 0;
u32 LocalAddr = 0;
/** Write to all 8 PTP frame templates */
for (PtpBufferPointer= 0; PtpBufferPointer < 8; PtpBufferPointer++) {
/** Only write to selected buffers */
if ( ((BufferEnable >> PtpBufferPointer) & 0x1) == 0x1) {
LocalAddr = (PtpBufferPointer<<8) + XAVB_PTP_TX_SYNC_OFFSET;
/** Read the current value */
LocalData = XAvb_ReadPtpBuffer(BaseAddress,
LocalAddr,
PtpFieldAddress);
/** Only change the selected data bits */
LocalData = LocalData | (Data & DataBitEnable);
/** Write the updated value */
XAvb_WritePtpBuffer(BaseAddress,
LocalAddr,
PtpFieldAddress,
LocalData);
#ifdef DEBUG_XAVB_LEVEL3
xil_printf("\r\nWriteToMultipleTxPtpFrames read/mod(PtpBufferPointer = %x) ", PtpBufferPointer);
xil_printf("\r\n PTPAddress = %x ", LocalAddr);
xil_printf("\r\n PtpFieldAddress = %x ", PtpFieldAddress);
xil_printf("\r\n LocalData = %x ", LocalData);
#endif
}
}
}
/****************************************************************************/
/**
*
* This function switches the bytes in a 4-byte word, swapping the MSB for the
* LSB, and vice-versa.
*
* @param Data is the 4-byte input data word
*
* @return The input data word with the bytes swapped (most significant down to
* least significant
*
* @note None.
*
*****************************************************************************/
u32 XAvb_ReorderWord(u32 Data) {
u32 ReOrder = 0;
ReOrder = (Data & 0x000000FF) << 24;
ReOrder = ReOrder | ((Data & 0x0000FF00) << 8);
ReOrder = ReOrder | ((Data & 0x00FF0000) >> 8);
ReOrder = ReOrder | ((Data & 0xFF000000) >> 24);
return ReOrder;
}
/****************************************************************************/
/**
*
* A function to increment the sequenceId in a PTP frame template
*
* @param BaseAddress is the base address of the device
* @param PtpFrameBaseAddress is the base address of the TX PTP Buffer whose
* SequenceID is to be incremented
*
* @return None. But the relevant TX PTP Packet Buffer is written to with the
* updated SequenceID
*
* @note None.
*
*****************************************************************************/
u32 XAvb_IncSequenceId(u32 BaseAddress, u32 PtpFrameBaseAddress) {
u32 BufferWord = 0;
u32 SequenceId = 0;
/** Read the 32-bit BufferWord containing the SequenceId from the PTP buffer */
BufferWord = XAvb_ReadPtpBuffer(BaseAddress,
PtpFrameBaseAddress,
XAVB_PTP_TX_PKT_SEQUENCEID_OFFSET);
/** Swap byte order into correct binary and increment the SequenceId */
SequenceId = XAvb_ReorderWord(BufferWord) + 0x10000;
/** Swap back the byte order into frame storage order */
SequenceId = XAvb_ReorderWord(SequenceId);
/** Write the 32-bit BufferWord variable containing the updated SequenceId */
XAvb_WritePtpBuffer(BaseAddress,
PtpFrameBaseAddress,
XAVB_PTP_TX_PKT_SEQUENCEID_OFFSET,
SequenceId);
return SequenceId;
}
/****************************************************************************/
/**
*
* The software drivers are kept simple by only requesting a single PTP frame to
* be transmitted at a time. This function checks (and if necessary waits)
* until the previously request PTP frame has been transmitted.
*
* @param InstancePtr is a pointer to the XAvb instance to be worked on
*
* @return None.
**
* @note None.
*
*****************************************************************************/
void XAvb_WaitOnTxPtpQueue(XAvb * InstancePtr) {
u32 TxPtpType = 0;
do {
/** Wait until any queued PTP frame has been transmitted. This is a
* software safety feature, not a hardware restriction */
TxPtpType = (XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_CONTROL_OFFSET)
& XAVB_PTP_TX_WAIT_ALL_FRAMES_MASK);
} while(TxPtpType != 0);
}
/****************************************************************************/
/**
*
* A function to format then request the transmission of a PTP Announce Packet
*
* @param InstancePtr is a pointer to the XAvb instance to be worked on
*
* @return None. But the relevant Tx PTP Packet Buffer is written to with the
* updated PTP fields, and then the Tx PTP Packet Buffer Control
* Register is written to request the frame transmission.
*
* @note None.
*
*****************************************************************************/
void XAvb_MasterSendAnnounce(XAvb * InstancePtr) {
u32 Unused = 0;
/** Wait until there are no PTP frames to be transmitted */
XAvb_WaitOnTxPtpQueue(InstancePtr);
/** Increment the sequenceId */
Unused = XAvb_IncSequenceId(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET);
#ifdef DEBUG_XAVB_LEVEL3
xil_printf("\r\n--------------------------------");
xil_printf("\r\n---XAvb_MasterSendAnnounce()----");
xil_printf("\r\n--------------------------------");
xil_printf("\r\nsequenceId is %x", Unused);
Unused = XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_RTC_NANOSEC_VALUE_OFFSET);
Unused = XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_RTC_SEC_LOWER_VALUE_OFFSET);
xil_printf("\r\nSeconds %d", Unused);
#endif
/** Send the Announce Frame! */
XAvb_WriteReg(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_CONTROL_OFFSET,
XAVB_PTP_TX_SEND_ANNOUNCE_FRAME_MASK);
}
/****************************************************************************/
/**
*
* A function to format then request the transmission of a PTP Sync Packet
*
* @param InstancePtr is a pointer to the XAvb instance to be worked on
*
* @return None. But the relevant Tx PTP Packet Buffer is written to with the
* updated PTP fields, and then the Tx PTP Packet Buffer Control
* Register is written to request the frame transmission.
*
* @note None.
*
*****************************************************************************/
void XAvb_MasterSendSync(XAvb * InstancePtr) {
u32 Epoch = 0;
u32 Sec = 0;
u32 SequenceId = 0;
u32 BufferWord = 0;
/** Wait until there are no PTP frames to be transmitted */
XAvb_WaitOnTxPtpQueue(InstancePtr);
/** Increment the sequenceId in the Sync frame */
SequenceId = XAvb_IncSequenceId(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_SYNC_OFFSET);
/** Read the current RTC Offset values */
InstancePtr->PtpRecords.Nanosec
= XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_RTC_NANOSEC_VALUE_OFFSET);
Sec = XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_RTC_SEC_LOWER_VALUE_OFFSET);
Epoch = XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_RTC_SEC_UPPER_VALUE_OFFSET);
/** Send the Sync Frame! */
XAvb_WriteReg(InstancePtr->Config.BaseAddress, XAVB_PTP_TX_CONTROL_OFFSET,
XAVB_PTP_TX_SEND_SYNC_FRAME_MASK);
/** Now some pre-work on the Follow-Up Frame
*-----------------------------------------
*
* Write the same sequenceId to the Follow-up frame */
BufferWord = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_SEQUENCEID_OFFSET);
BufferWord = (BufferWord & 0xFFFF0000) | (SequenceId & 0x0000FFFF);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_SEQUENCEID_OFFSET,
BufferWord);
/** Format the Timestamp (RTC) into correct byte positioning.
* Note: this is how the Timestamp is stored in
* the PTP frame itself (transmitted MSB of Epoch first):
*
* |----------------|----------------|----------------|----------------|
* | seconds[23:16] | seconds[31:24] | epoch[7:0] | epoch[15:8] |
* |----------------|----------------|----------------|----------------|
* | nanosec[23:16] | nanosec[31:24] | seconds[7:0] | seconds[15:8] |
* |----------------|----------------|----------------|----------------|
* | 0's | 0's | nanosec[7:0] | nanosec[15:8] |
* |----------------|----------------|----------------|----------------|
*/
BufferWord = XAvb_ReorderWord((Epoch<<16) | (Sec>>16));
/** Write the Timestamp (RTC) to the Follow-up frame */
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_UPPER_OFFSET,
BufferWord);
BufferWord = XAvb_ReorderWord((Sec<<16) |
(InstancePtr->PtpRecords.Nanosec>>16));
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_MID_OFFSET,
BufferWord);
BufferWord = XAvb_ReorderWord(InstancePtr->PtpRecords.Nanosec<<16);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_LOWER_OFFSET,
BufferWord);
#ifdef DEBUG_XAVB_LEVEL3
xil_printf("\r\n--------------------------------");
xil_printf("\r\n-----XAvb_MasterSendSync()------");
xil_printf("\r\n--------------------------------");
xil_printf("\r\n--------------RTC---------------");
xil_printf("\r\nRTC nanosec field is %x", InstancePtr->PtpRecords.Nanosec);
xil_printf("\r\nRTC seconds field is %x", Sec);
xil_printf("\r\nRTC epoch field is %x", Epoch);
xil_printf("\r\n--------------------------------");
xil_printf("\r\nsequenceId is %x", SequenceId);
#endif
}
/****************************************************************************/
/**
*
* A function to format then request the transmission of a PTP Follow-Up Packet
*
* @param InstancePtr is a pointer to the XAvb instance to be worked on
*
* @return None. But the relevant Tx PTP Packet Buffer is written to with the
* updated PTP fields, and then the Tx PTP Packet Buffer Control
* Register is written to request the frame transmission.
*
* @note None.
*
*****************************************************************************/
void XAvb_MasterSendFollowUp(XAvb * InstancePtr) {
u32 Timestamp = 0;
u32 NsOffset = 0;
u32 CorrectionField = 0;
u32 BufferWord = 0;
/** Wait until there are no PTP frames to be transmitted */
XAvb_WaitOnTxPtpQueue(InstancePtr);
/** Read the current RTC offset */
NsOffset = XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_RTC_NANOSEC_OFFSET);
/** Read the Timestamp and adjust it for the MAC transmit latency */
Timestamp = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_SYNC_OFFSET,
XAVB_PTP_PKT_CAPTURED_TIMESTAMP_OFFSET)
+ XAVB_TX_MAC_LATENCY_IN_NS;
/** Adjust the Timestamp with current RTC ns offset */
Timestamp = Timestamp + NsOffset;
if (Timestamp >= XAVB_ONE_SECOND) {
Timestamp = Timestamp - XAVB_ONE_SECOND;
}
/** Calculate the Correction Field */
CorrectionField = Timestamp - InstancePtr->PtpRecords.Nanosec;
if (CorrectionField >= XAVB_ONE_SECOND) {
CorrectionField = CorrectionField + XAVB_ONE_SECOND;
}
/** Format the Correction Field into correct byte positioning for PTP frame
* storage in the buffer
*/
BufferWord = XAvb_ReorderWord(CorrectionField);
/** Write the Correction Field to the Follow Up frame */
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_CORRECTION_FIELD_OFFSET,
BufferWord);
/** Send the Follow Up Frame! */
XAvb_WriteReg(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_CONTROL_OFFSET,
XAVB_PTP_TX_SEND_FOLLOWUP_FRAME_MASK);
#ifdef DEBUG_XAVB_LEVEL3
xil_printf("\r\n--------------------------------");
xil_printf("\r\n---XAvb_MasterSendFollowUp()----");
xil_printf("\r\n--------------------------------");
xil_printf("\r\nTimestamp is %x", Timestamp);
xil_printf("\r\nRTC nano seconds field is %x",
InstancePtr->PtpRecords.Nanosec);
xil_printf("\r\nCorrection Field %x", CorrectionField);
#endif
}
/****************************************************************************/
/**
*
* A function to format then request the transmission of a PTP PDelay Request
* Packet
*
* @param InstancePtr is a pointer to the XAvb instance to be worked on
*
* @return None. But the relevant Tx PTP Packet Buffer is written to with the
* updated PTP fields, and then the Tx PTP Packet Buffer Control
* Register is written to request the frame transmission.
*
* @note None.
*
*****************************************************************************/
void XAvb_SendPDelayReq(XAvb * InstancePtr) {
u32 SequenceId;
/** Wait until there are no PTP frames to be transmitted */
XAvb_WaitOnTxPtpQueue(InstancePtr);
/** Increment the SequenceId */
SequenceId = XAvb_IncSequenceId(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYREQ_OFFSET)
& 0x0000FFFF;
#ifdef DEBUG_XAVB_LEVEL3
xil_printf("\r\n--------------------------------");
xil_printf("\r\n------XAvb_SendPDelayReq()------");
xil_printf("\r\n--------------------------------");
xil_printf("\r\nsequenceId is %x", SequenceId);
#endif
/** Send the PDelayReq Frame! */
XAvb_WriteReg(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_CONTROL_OFFSET,
XAVB_PTP_TX_SEND_PDELAYREQ_FRAME_MASK);
/** Wait for the frame to be transmitted */
XAvb_WaitOnTxPtpQueue(InstancePtr);
/** Capture the Timestamp for Tx of PDelayReq (t1) and adjust it for MAC
* transmit latency */
InstancePtr->PtpRecords.PDelayTimestampT1
= XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYREQ_OFFSET,
XAVB_PTP_PKT_CAPTURED_TIMESTAMP_OFFSET)
+ XAVB_TX_MAC_LATENCY_IN_NS;
/** Capture the SequenceID of the the PDelayReq */
InstancePtr->SequenceIdRecords.PDelayReqSequenceId =
XAvb_ReorderWord(XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYREQ_OFFSET,
XAVB_PTP_TX_PKT_SEQUENCEID_OFFSET)) >> 16;
}
/****************************************************************************/
/**
*
* A function to format then request the transmission of a PTP PDelay Response
* Packet
*
* @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 None. But the relevant Tx PTP Packet Buffer is written to with the
* updated PTP fields, and then the Tx PTP Packet Buffer Control
* Register is written to request the frame transmission.
*
* @note None.
*
*****************************************************************************/
void XAvb_SendPDelayResp(XAvb * InstancePtr,
u32 PtpFrameBaseAddr) {
u32 SequenceId = 0;
u32 TimestampT2 = 0;
u32 BufferWord = 0;
u32 NanoSec = 0;
u32 Seconds = 0;
u32 Epoch = 0;
u32 CopyPortId = 0;
u32 CopyPortId1 = 0;
/** Wait until there are no PTP frames to be transmitted */
XAvb_WaitOnTxPtpQueue(InstancePtr);
/** Format the Timestamp
*---------------------
*
* Capture the current Synchronised time */
NanoSec = XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_RTC_NANOSEC_VALUE_OFFSET);
Seconds = XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_RTC_SEC_LOWER_VALUE_OFFSET);
Epoch = XAvb_ReadReg(InstancePtr->Config.BaseAddress,
XAVB_RTC_SEC_UPPER_VALUE_OFFSET);
/** Read the current RTC offset */
InstancePtr->PtpRecords.NsOffsetForPDelayResp =
XAvb_ReadReg(InstancePtr->Config.BaseAddress, XAVB_RTC_NANOSEC_OFFSET);
/** Read the TimestampT2 for PDelayReq reception and adjust it for MAC
* receive latency */
TimestampT2 = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_PKT_CAPTURED_TIMESTAMP_OFFSET)
- XAVB_RX_MAC_LATENCY_IN_NS;
/** The TimestampT2 was captured using syntonised ns time. We need to
* convert this into synchronised time by adding on the current offset */
TimestampT2 = TimestampT2 + InstancePtr->PtpRecords.NsOffsetForPDelayResp;
/** Check for ns wrap-around condition */
if (TimestampT2 >= XAVB_ONE_SECOND) {
TimestampT2 = TimestampT2 - XAVB_ONE_SECOND;
}
/** Even though we read the RTC value at the beginning of this
* function, there would have been processing delay between the
* actual reception (and timestamping) of the PDelayReq frame and the
* start of this function. During this time, the 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 (NanoSec < TimestampT2) {
/** NanoSec has wrapped since timestamp was taken so decrement the
* Seconds field */
if (Seconds == 0x00000000) {
Epoch = Epoch - 0x1;
}
Seconds = Seconds - 0x1;
}
/** Format the Timestamp (t2) into correct byte positioning for PTP frame
* storage, then write the Timestamp (t2) to the PDelayResp frame
*/
BufferWord = XAvb_ReorderWord((Epoch<<16) | (Seconds>>16));
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_UPPER_OFFSET,
BufferWord);
BufferWord = XAvb_ReorderWord((Seconds<<16) | (TimestampT2>>16));
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_MID_OFFSET,
BufferWord);
BufferWord = XAvb_ReorderWord(TimestampT2<<16);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_LOWER_OFFSET,
BufferWord);
/** Format the SequenceId
*----------------------
*
* Set the SequenceId in the PDelayResp and PDelayRespFollowUp frame to be
* that of the received PDelayReq frame */
SequenceId = (XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_SEQUENCEID_OFFSET))
& 0x0000FFFF;
BufferWord = XAVB_PDELAY_LOG_MEAN_MESSAGE_INT | SequenceId;
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_OFFSET,
XAVB_PTP_TX_PKT_SEQUENCEID_OFFSET,
BufferWord);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_SEQUENCEID_OFFSET,
BufferWord);
/** Format the sourcePortIdentity
*------------------------------
*
* Copy the sourcePortIdentity field from the PDelayReq into the PDelayResp
* and PDelayRespFollowUp frame
*/
CopyPortId = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_PORTID_UPPER_OFFSET);
CopyPortId &= 0xffff0000;
CopyPortId1 = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_OFFSET,
XAVB_PTP_TX_PKT_REQ_PORTID_UPPER_OFFSET);
CopyPortId |= (CopyPortId1 & 0x0000ffff);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_OFFSET,
XAVB_PTP_TX_PKT_REQ_PORTID_UPPER_OFFSET,
CopyPortId);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_REQ_PORTID_UPPER_OFFSET,
CopyPortId);
CopyPortId = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_PORTID_MID_OFFSET);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_OFFSET,
XAVB_PTP_TX_PKT_REQ_PORTID_MID_OFFSET,
CopyPortId);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_REQ_PORTID_MID_OFFSET,
CopyPortId);
CopyPortId = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_PORTID_LOWER_OFFSET);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_OFFSET,
XAVB_PTP_TX_PKT_REQ_PORTID_LOWER_OFFSET,
CopyPortId);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_REQ_PORTID_LOWER_OFFSET,
CopyPortId);
/** Send the PDelayResp Frame!
*---------------------------*/
XAvb_WriteReg(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_CONTROL_OFFSET,
XAVB_PTP_TX_SEND_PDELAYRESP_FRAME_MASK);
#ifdef DEBUG_XAVB_LEVEL3
xil_printf("\r\n--------------------------------");
xil_printf("\r\n------XAvb_SendPDelayResp()-----");
xil_printf("\r\n--------------------------------");
xil_printf("\r\nTimestampT2 is %x", TimestampT2);
#endif
}
/****************************************************************************/
/**
*
* A function to format then request the transmission of a PTP PDelay Response
* Follow-Up Packet
*
* @param InstancePtr is a pointer to the XAvb instance to be worked on
*
* @return None. But the relevant Tx PTP Packet Buffer is written to with the
* updated PTP fields, and then the Tx PTP Packet Buffer Control
* Register is written to request the frame transmission.
*
* @note None.
*
*****************************************************************************/
void XAvb_SendPDelayRespFollowUp(XAvb * InstancePtr) {
u32 TimestampT3 = 0;
u32 BufferWordA = 0;
u32 BufferWordB = 0;
XAvb_RtcFormat Rtc[1];
/** Wait until there are no PTP frames to be transmitted */
XAvb_WaitOnTxPtpQueue(InstancePtr);
/** Format the Timestamp
*---------------------*/
/** Capture the current Synchronised time */
XAvb_ReadRtc(InstancePtr->Config.BaseAddress, Rtc);
/** Read the TimestampT3 for PDelayResp transmission and adjust it for MAC
* transmit latency */
TimestampT3 = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_OFFSET,
XAVB_PTP_PKT_CAPTURED_TIMESTAMP_OFFSET)
+ XAVB_TX_MAC_LATENCY_IN_NS;
/** The TimestampT3 was captured using syntonised ns time. We need to
* convert this into synchronised time by adding on the ns offset. We
* use the same offset here as for the PDelayResp frame since if a
* ns offset change had been made between PDelayResp and ,
* PDelayRespFollowUp, this would result in an error in the link delay
* measurement. */
TimestampT3 = TimestampT3 + InstancePtr->PtpRecords.NsOffsetForPDelayResp;
/** Check for ns wrap-around condition */
if (TimestampT3 >= XAVB_ONE_SECOND) {
TimestampT3 = TimestampT3 - XAVB_ONE_SECOND;
}
/** Even though we read the RTC value at the beginning of this
* function, there would have been processing delay between the
* actual reception (and timestamping) of the PDelayReq frame and the
* start of this function. During this time, the 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 (Rtc->NanoSeconds < TimestampT3) {
/** nanosec has wrapped since timestamp was taken so decrement the
* seconds field */
if (Rtc->SecondsLower == 0x00000000) {
Rtc->SecondsUpper = Rtc->SecondsUpper - 0x1;
}
Rtc->SecondsLower = Rtc->SecondsLower - 0x1;
}
/** Format the Timestamp (t3) into correct byte positioning for PTP frame
* storage, the write the Timestamp (t3) to the PDelayRespFollowUp frame
*/
BufferWordA = XAvb_ReorderWord((Rtc->SecondsUpper<<16) |
(Rtc->SecondsLower>>16));
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_UPPER_OFFSET,
BufferWordA);
BufferWordA = XAvb_ReorderWord((Rtc->SecondsLower<<16) | (TimestampT3>>16));
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_MID_OFFSET,
BufferWordA);
BufferWordA = XAvb_ReorderWord(TimestampT3<<16);
BufferWordB = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_LOWER_OFFSET);
BufferWordA &= 0x0000ffff;
BufferWordA |= (BufferWordB & 0xffff0000);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYRESP_FOLLOW_UP_OFFSET,
XAVB_PTP_TX_PKT_TIMESTAMP_LOWER_OFFSET,
BufferWordA);
/** Send the PDelayRespFollowUp Frame!
*----------------------------------- */
XAvb_WriteReg(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_CONTROL_OFFSET,
XAVB_PTP_TX_SEND_PDELAYRESPFOLLOWUP_FRAME_MASK);
#ifdef DEBUG_XAVB_LEVEL3
xil_printf("\r\n--------------------------------");
xil_printf("\r\n--XAvb_SendPDelayRespFollowUp()-");
xil_printf("\r\n--------------------------------");
xil_printf("\r\nReading TimestampT3 from address %x",
(InstancePtr->Config.BaseAddress + XAVB_PTP_TX_PDELAYRESP_OFFSET
+ XAVB_PTP_PKT_CAPTURED_TIMESTAMP_OFFSET));
xil_printf("\r\nTimestampT3 is %x", TimestampT3);
#endif
}
/****************************************************************************/
/**
*
* A function to check that various fields in the received frame contain the
* expected values which define it as a valid AVB PTP frame. If this check does
* not pass then the frame should not be decoded and used.
*
* @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 An updated True/False decision as to whether this received frame
* really is a valid PTP type.
*
* @note None.
*
*****************************************************************************/
u32 XAvb_IsRxFramePTP(XAvb * InstancePtr,
u32 PtpFrameBaseAddr) {
u32 FrameIsPTP;
u32 FrameField;
/** Start by assuming that it is a valid PTP frame */
FrameIsPTP = 1;
/** Perform a 32-bit read from the relevant position in the frame */
FrameField = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_TYPE_OFFSET);
FrameField = XAvb_ReorderWord(FrameField);
/** Check the Length/Type field for a valid Ethertype */
if ((FrameField >>16) != XAVB_PTP_ETHERTYPE) {
FrameIsPTP = 0;
#ifdef DEBUG_XAVB_LEVEL1
xil_printf("\r\nXAvb_IsRxFramePTP(): Bad Ethertype: %x", (FrameField >>16));
#endif
}
/** Check the versionPTP */
if ((FrameField & 0xF) != XAVB_PTP_VERSION_PTP) {
FrameIsPTP = 0;
#ifdef DEBUG_XAVB_LEVEL1
xil_printf("\r\nXAvb_IsRxFramePTP():Bad versionPTP:%x", (FrameField & 0xF));
#endif
}
return FrameIsPTP;
}
/****************************************************************************/
/**
*
* A function to decode a received PTP Sync Packet
*
* @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 None.
*
* @note None.
*
*****************************************************************************/
void XAvb_DecodeRxSync(XAvb * InstancePtr,
u32 PtpFrameBaseAddr) {
u32 ReadWord = 0;
XAvb_PortIdentity syncPortID;
/** Read sourcePortIdentity from packet */
XAvb_GetPortIdentity(InstancePtr->Config.BaseAddress, PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_PORTID_UPPER_OFFSET, &syncPortID);
/** Only decode if configured for a slave and if SourcePortID is that of the
* RTC Clock Master */
if ( (InstancePtr->CurrentBmc.IAmTheRtcMaster == 0) &&
XAvb_ComparePortIdentity(InstancePtr->Config.BaseAddress,
InstancePtr->CurrentBmc.SourcePortIdentity,
syncPortID) ) {
/** Reset Sync Interval Counter as we have received a valid Sync */
InstancePtr->PtpCounters.CounterSyncInterval = 0;
/** Capture the local Timestamp for receipt of this frame and adjust it for
* MAC receive latency */
InstancePtr->PtpRecords.SlaveSyncTimestamp
= XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_PKT_CAPTURED_TIMESTAMP_OFFSET)
- XAVB_RX_MAC_LATENCY_IN_NS;
/** Capture the Sync SequenceID */
ReadWord = XAvb_ReorderWord(XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_SEQUENCEID_OFFSET));
InstancePtr->SequenceIdRecords.SyncSequenceId = (ReadWord >> 16);
/** Capture the logMeanMessageInterval and convert into a useful duration
* (NOTE: there is an implicit conversion from u32 to char here)
*/
InstancePtr->latestMDSyncReceive.logMessageInterval = ReadWord & 0x000000ff;
InstancePtr->latestMDSyncReceive.SyncIntervalDuration =
XAvb_ConvertLogMeanToDuration(InstancePtr->latestMDSyncReceive.logMessageInterval);
/** We don't need to capture the correction field - unless we want to check that it is 0.*/
} else {
xil_printf("\r\nXAvb_DecodeRxSync()");
xil_printf("\r\nSync ignored due to unmatched SourcePortID");
#ifdef DEBUG_XAVB_LEVEL1
xil_printf("\r\nInstancePtr->CurrentBmc.SourcePortIdentity.ClockIdentityUpper = %x",InstancePtr->CurrentBmc.SourcePortIdentity.ClockIdentityUpper);
xil_printf("\r\nInstancePtr->CurrentBmc.SourcePortIdentity.ClockIdentityLower = %x",InstancePtr->CurrentBmc.SourcePortIdentity.ClockIdentityLower);
xil_printf("\r\nInstancePtr->CurrentBmc.SourcePortIdentity.PortNumber = %x\r\n",InstancePtr->CurrentBmc.SourcePortIdentity.PortNumber);
xil_printf("\r\nsyncPortID.ClockIdentityUpper = %x",syncPortID.ClockIdentityUpper);
xil_printf("\r\nsyncPortID.ClockIdentityLower = %x",syncPortID.ClockIdentityLower);
xil_printf("\r\nsyncPortID.PortNumber = %x\r\n",syncPortID.PortNumber);
#endif
}
}
/****************************************************************************/
/**
*
* A function to decode a received PTP Follow-up Packet
*
* @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 None.
*
* @note None.
*
*****************************************************************************/
void XAvb_DecodeRxFollowUp(XAvb * InstancePtr,
u32 PtpFrameBaseAddr) {
XAvb_PortIdentity followUpPortID;
/** Read sourcePortIdentity from packet */
XAvb_GetPortIdentity(InstancePtr->Config.BaseAddress, PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_PORTID_UPPER_OFFSET, &followUpPortID);
/** Only decode if configured for a slave and if SA is that of the RTC
* Clock Master */
if ( (InstancePtr->CurrentBmc.IAmTheRtcMaster == 0) &&
XAvb_ComparePortIdentity(InstancePtr->Config.BaseAddress,
InstancePtr->CurrentBmc.SourcePortIdentity,
followUpPortID) ) {
/** Capture the Follow Up SequenceID */
InstancePtr->SequenceIdRecords.FollowUpSequenceId
= XAvb_ReorderWord(XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_SEQUENCEID_OFFSET))
>> 16;
/** SequenceID in Follow Up Frame should always match that of the
* Sync Frame */
if (InstancePtr->SequenceIdRecords.FollowUpSequenceId
== InstancePtr->SequenceIdRecords.SyncSequenceId) {
/** Capture the correction field from follow up frame */
InstancePtr->PtpRecords.MasterCorrectionField
= XAvb_ReorderWord(XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_CORRECTION_FIELD_OFFSET));
/** Perform the Course RTC Offset correction for every Sync /
* FollowUp pair */
XAvb_CalcRtcOffset(InstancePtr,
PtpFrameBaseAddr);
/** Every n Sync / FollowUp pairs, we are going to calculate a
* corrected increment rate of RTC */
if ((InstancePtr->PtpCounters.CounterSyncEvents & 0xF)
== (XAVB_NUM_SYNC_FU_PAIR_CALC_RTC_INCREMENT - 1)) {
/** Reset the CounterSyncEvents Counter */
InstancePtr->PtpCounters.CounterSyncEvents = 0x0;
/** Capture the Sequence ID of the Follow Up frame */
InstancePtr->SequenceIdRecords.NewSyncSequenceId
= XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_SEQUENCEID_OFFSET);
InstancePtr->SequenceIdRecords.NewSyncSequenceId
= (XAvb_ReorderWord
(InstancePtr->SequenceIdRecords.NewSyncSequenceId)
) >> 16;
/** Perform the RTC increment rate adjustment calculation */
XAvb_UpdateRtcIncrement(InstancePtr);
/** Sample Sync Frame Time sent (as estimated by the slave) for
* comparison in ten more repetition's time */
InstancePtr->PtpRecords.OldSlaveTime
= InstancePtr->PtpRecords.NewSlaveTime;
/** Sample Sync Frame Time sent (as calculated by the master)
* for comparison in ten more repetition's time */
InstancePtr->PtpRecords.OldMasterTime
= InstancePtr->PtpRecords.NewMasterTime;
/** Sample the current Follow Up Sequence ID for comparison in
* ten more repetition's time */
InstancePtr->SequenceIdRecords.OldSyncSequenceId
= InstancePtr->SequenceIdRecords.NewSyncSequenceId;
} else {
InstancePtr->PtpCounters.CounterSyncEvents
= InstancePtr->PtpCounters.CounterSyncEvents + 1;
}
} else {
xil_printf("SequenceIDs on RxFollowup don't match.\r\n");
}
} else {
xil_printf("\r\nXAvb_DecodeRxFollowUp()");
xil_printf("\r\nFollowUp ignored due to unmatched SourcePortID\r\n");
}
}
/****************************************************************************/
/**
*
* A function to decode a received PDelayResp Packet
*
* @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 None.
*
* @note None.
*
*****************************************************************************/
void XAvb_DecodeRxPDelayResp(XAvb * InstancePtr,
u32 PtpFrameBaseAddr) {
/** Have we already seen a PDelayResp since the last
* PDelayReq was sent? If so, ignore the packet */
if( InstancePtr->StateMachineData.rcvdPDelayResp ) {
xil_printf("Error: already saw a PDelayResp since the last PDelayReq was sent\r\n");
return;
}
/** Find the ClockIdentity of the Sender */
XAvb_GetPortIdentity(InstancePtr->Config.BaseAddress, PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_PORTID_UPPER_OFFSET,
&InstancePtr->StateMachineData.respPortIdentity);
/** Is the PDelayResp message from ourself? If so, the Peer
* is most likely a dumb hub and should be considered not
* ASCapable */
if( XAvb_ComparePortIdentity(InstancePtr->Config.BaseAddress,
InstancePtr->portIdLocal,
InstancePtr->StateMachineData.respPortIdentity) ) {
XAvb_ChangePeerASCapability(InstancePtr, 0);
xil_printf("\r\nXAvb_DecodeRxPDelayResp():The peer is no longer ASCapable ");
xil_printf("\r\nXAvb_DecodeRxPDelayResp():Saw a PDelayResp from myself\r\n");
return;
}
/** Capture the requestingPortIdentity */
XAvb_GetPortIdentity(InstancePtr->Config.BaseAddress, PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_REQ_PORTID_UPPER_OFFSET,
&InstancePtr->StateMachineData.respReqPortIdentity);
/** Capture the PDelayResp SequenceID */
InstancePtr->SequenceIdRecords.PDelayRespSequenceId =
XAvb_ReorderWord(XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_SEQUENCEID_OFFSET)) >> 16;
/** Verify that the requestingPortIdentity matches our
* portIdentity */
if( !XAvb_ComparePortIdentity(InstancePtr->Config.BaseAddress,
InstancePtr->portIdLocal,
InstancePtr->StateMachineData.respReqPortIdentity) ) {
xil_printf("Error: PDelayResp reqPortID doesn't match our portID\r\n");
return;
}
/** Only process if the received frame's sequenceId matches
* the sequenceId sent in the last pDelay_Req packet */
if( (InstancePtr->SequenceIdRecords.PDelayReqSequenceId ==
InstancePtr->SequenceIdRecords.PDelayRespSequenceId) ) {
/** Mark this as a valid PDelayResp packet */
InstancePtr->StateMachineData.rcvdPDelayResp = 1;
/** Capture timestamp for receipt time of PDelayReq at Master (t2) */
InstancePtr->PtpRecords.PDelayTimestampT2 =
XAvb_CaptureNanoSec(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr);
/** Capture timestamp for receipt time of PDelayResp at Slave (t4) and adjust
* it for MAC receive latency */
InstancePtr->PtpRecords.PDelayTimestampT4 =
XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_PKT_CAPTURED_TIMESTAMP_OFFSET)
- XAVB_RX_MAC_LATENCY_IN_NS;
} else {
xil_printf("Error: PDelayResp seqID's don't match\r\n");
}
}
/****************************************************************************/
/**
*
* A function to decode a received PDelayRespFollowUp Packet
*
* @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 None.
*
* @note None.
*
*****************************************************************************/
void XAvb_DecodeRxPDelayRespFollowUp(XAvb * InstancePtr,
u32 PtpFrameBaseAddr) {
XAvb_PortIdentity portId;
/** Has a valid PDelayResp packet been received since the
* last PDelayReq packet was sent? */
if( !InstancePtr->StateMachineData.rcvdPDelayResp ) {
/*xil_printf("Error: Received a PDelayRespFollowUp before receiving a PDelayResp\r\n");*/
return;
}
/** Capture the PDelayRespFollowUp SequenceID */
InstancePtr->SequenceIdRecords.PDelayFollowUpSequenceId =
XAvb_ReorderWord(XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_SEQUENCEID_OFFSET)) >> 16;
/** Get the sourcePortIdentity of the sender */
XAvb_GetPortIdentity(InstancePtr->Config.BaseAddress, PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_PORTID_UPPER_OFFSET,
&portId);
/** The sourcePortIdentity of the PDelayRespFollowUp should
* match that of the last PDelayResp packet received */
if( !XAvb_ComparePortIdentity(InstancePtr->Config.BaseAddress,
portId,
InstancePtr->StateMachineData.respPortIdentity) ) {
xil_printf("Error: sourcePortIdentity of PDelayRespFollowUp doesn't match PDelayResp\r\n");
return;
}
/** Get the requestingPortIdentity of the sender */
XAvb_GetPortIdentity(InstancePtr->Config.BaseAddress, PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_REQ_PORTID_UPPER_OFFSET,
&portId);
/** The requestingPortIdentity of the PDelayRespFollowUp should
* match that of the last PDelayResp packet received */
if( !XAvb_ComparePortIdentity(InstancePtr->Config.BaseAddress,
portId,
InstancePtr->StateMachineData.respReqPortIdentity) ) {
xil_printf("Error: reqPortID of PDelayRespFollowUp doesn't match PDelayResp\r\n");
return;
}
/** SequenceID of PDelayRespFollowUp Frame should always match that of
* the PDelayResp Frame and the original PDelayReq Frame. */
if (InstancePtr->SequenceIdRecords.PDelayFollowUpSequenceId ==
InstancePtr->SequenceIdRecords.PDelayRespSequenceId) {
/** Mark this as a valid PDelayRespFollowUp packet */
InstancePtr->StateMachineData.rcvdPDelayRespFollowUp = 1;
/** Capture the timestamp for transmit time of PDelayResp at Master
* (t3) */
InstancePtr->PtpRecords.PDelayTimestampT3 =
XAvb_CaptureNanoSec(InstancePtr->Config.BaseAddress, PtpFrameBaseAddr);
/** Now we know t1, t2, t3 and t4, calculate the link delay */
XAvb_CalcDelay(InstancePtr);
} else {
xil_printf("Error: seqID of PDelayRespFollowUp doesn't match PDelayResp\r\n");
}
}
/****************************************************************************/
/**
*
* A function to decode a received Signalling Packet and modify the TX PTP
* Buffers based on the requested values.
*
* @param InstancePtr is a pointer to the XAvb instance to be worked on
* @param PtpFrameBaseAddr is the base address of the received Signaling Packet
* in the Rx PTP Packet Buffer
*
* @return None.
*
* @note None.
*
*****************************************************************************/
void XAvb_DecodeRxSignaling(XAvb * InstancePtr, u32 PtpFrameBaseAddr) {
u32 ReadData;
char currentInterval;
/** Read the requested logMeanMessage durations from the Signalling frame */
ReadData = XAvb_ReorderWord(XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_RX_PKT_SIGNALING_DELAY_INTERVAL_OFFSET));
/** update linkDelayInterval */
currentInterval = (ReadData >> 24);
switch( currentInterval ) {
case (-128):
/** don't change the interval */
break;
/** currently only support the default value */
case XAVB_DEFAULT_LOG_MEAN_PDELAY_REQ_INTERVAL:
case 126:
/** set the interval to initial value */
InstancePtr->SignallingFrameData.LinkDelayIntervalDuration =
XAvb_UpdateIntervalDuration(InstancePtr->SignallingFrameData.LinkDelayIntervalDuration,
XAVB_DEFAULT_LOG_MEAN_PDELAY_REQ_INTERVAL);
/** Update logMeanMessageInterval in the pre-loaded TX PDELAYREQ message buffer */
XAvb_UpdateLogMeanMessageInterval(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PDELAYREQ_OFFSET,
InstancePtr->SignallingFrameData.LinkDelayIntervalDuration);
break;
case 127:
/** stop sending pDelay messages */
InstancePtr->SignallingFrameData.LinkDelayIntervalDuration = XAVB_PKT_TYPE_DISABLED;
break;
default:
xil_printf( "Got a signalling message with an interval (%d) I don't support!\r\n", currentInterval);
}
/** update timeSyncInterval */
currentInterval = (ReadData >> 16);
switch( currentInterval ) {
case (-128):
/** don't change the interval */
break;
case XAVB_DEFAULT_LOG_MEAN_SYNC_INTERVAL:
case 126:
/** set the interval to initial value */
InstancePtr->SignallingFrameData.SyncIntervalDuration =
XAvb_UpdateIntervalDuration(InstancePtr->SignallingFrameData.SyncIntervalDuration,
XAVB_DEFAULT_LOG_MEAN_SYNC_INTERVAL);
/** Update logMeanMessageInterval in the pre-loaded TX SYNC message buffer */
XAvb_UpdateLogMeanMessageInterval(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_SYNC_OFFSET,
InstancePtr->SignallingFrameData.SyncIntervalDuration);
/** Update logMeanMessageInterval in the pre-loaded TX FOLLOW_UP message buffer */
XAvb_UpdateLogMeanMessageInterval(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_FOLLOW_UP_OFFSET,
InstancePtr->SignallingFrameData.SyncIntervalDuration);
break;
case 127:
/** stop sending sync messages */
InstancePtr->SignallingFrameData.SyncIntervalDuration = XAVB_PKT_TYPE_DISABLED;
break;
default:
xil_printf( "Got a signalling message with an interval (%d) I don't support!\r\n", currentInterval);
}
/** update announceInterval */
currentInterval = (ReadData >> 8);
switch( currentInterval ) {
case (-128):
/** don't change the interval */
break;
case XAVB_DEFAULT_LOG_MEAN_ANNOUNCE_INTERVAL:
case 126:
/** set the interval to initial value */
InstancePtr->SignallingFrameData.AnnounceIntervalDuration =
XAvb_UpdateIntervalDuration(InstancePtr->SignallingFrameData.AnnounceIntervalDuration,
XAVB_DEFAULT_LOG_MEAN_ANNOUNCE_INTERVAL);
/** Update logMeanMessageInterval in the pre-loaded TX ANNOUNCE message buffer */
XAvb_UpdateLogMeanMessageInterval(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
InstancePtr->SignallingFrameData.AnnounceIntervalDuration);
break;
case 127:
/** stop sending Announce messages */
InstancePtr->SignallingFrameData.AnnounceIntervalDuration = XAVB_PKT_TYPE_DISABLED;
break;
default:
xil_printf( "Got a signalling message with an interval (%d) I don't support!\r\n", currentInterval);
}
}
/****************************************************************************/
/**
*
* A function to update a PTP message Interval Duration (defined as a
* fraction of 128 seconds). If the endpoint cannot support a requested
* logMeanVal then do not perform the conversion - return the current value.
*
* @param currentIntervalDuration is the Interval Duration to be updated
* @param logMeanVal is the base2 value that is to be converted
*
* @return logMeanVal represented as a fraction of 128
*
* @note This endpoint only supports logMeanValues >=-7 and <=8.
*
*****************************************************************************/
u16 XAvb_UpdateIntervalDuration(u16 currentIntervalDuration,
char logMeanVal) {
if((logMeanVal >= XAVB_MIN_SUPPORTED_LOG_MEAN_INTERVAL) &&
(logMeanVal <= XAVB_MAX_SUPPORTED_LOG_MEAN_INTERVAL)) {
return XAvb_ConvertLogMeanToDuration(logMeanVal);
} else {
return currentIntervalDuration;
}
}
/****************************************************************************/
/**
*
* A function to convert a logMean (power of 2) value into a fraction of 128
* that is compatible with Signalling data.
*
* @param logMeanVal is the base2 value that is to be converted
* @return logMeanVal represented as a fraction of 128
*
* @note None.
*
*****************************************************************************/
u16 XAvb_ConvertLogMeanToDuration(char logMeanVal) {
u8 logMeanAbs;
logMeanAbs = (u8)(abs(logMeanVal));
return (logMeanVal < 0) ?
(128 >> logMeanAbs) :
(logMeanVal > 0) ?
(128 << logMeanAbs) :
128;
}
/****************************************************************************/
/**
*
* A function to convert a fraction of 128 value that is compatible with
* Signalling data into a logMean (power of 2) value;
*
* @param fractionalVal is the Signalling data value that is to be converted
* @return fractionalVal represented as logMean (power of 2) value
*
* @note None.
*
*****************************************************************************/
char XAvb_ConvertDurationToLogMean(u16 fractionalVal) {
char logMeanVal = 0;
u8 numShifts = 0;
/** just in case fractionalVal is not a power of 2, we'll
* only look at the most significant bit
* Count how many shifts it takes for most significant set bit
* to be in the highest (16th) bit location
*/
while( !(fractionalVal & 0x8000) ) {
fractionalVal <<= 1;
numShifts++;
}
/** logMeanVal = 0 = 2^0 = 128/128 would give us a numShifts
* result of 8, so 8 will be our base
*/
logMeanVal = 8 - numShifts;
return logMeanVal;
}
/****************************************************************************/
/**
*
* A function to update the logMeanMessageInterval field in a PTP packet
*
* @param BaseAddress is the base address of the device
* @param PtpFrameBaseAddr is the base address of the TX PTP Buffer to be updated
* @param intervalDuration is the "fraction of 128" value of the data to be written
*
* @return None. But the relevant Tx PTP Packet Buffer is written to with the
* updated logMeanMessageInterval
*
* @note None.
*
*****************************************************************************/
void XAvb_UpdateLogMeanMessageInterval(u32 BaseAddress,
u32 PtpFrameBaseAddr,
u16 intervalDuration) {
u32 ReadVal;
u8 logMean;
/** Convert intervalDuration to a logMean value */
logMean = (u8)XAvb_ConvertDurationToLogMean(intervalDuration);
/** Read the current fields */
ReadVal = XAvb_ReadPtpBuffer(BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_TX_PKT_SEQUENCEID_OFFSET);
/** Update just the logMeanMessageInterval field */
ReadVal = (ReadVal & 0x00ffffff) | (logMean << 24);
/** Write back */
XAvb_WritePtpBuffer(BaseAddress,
PtpFrameBaseAddr,
XAVB_PTP_TX_PKT_SEQUENCEID_OFFSET,
ReadVal);
}
/****************************************************************************/
/**
*
* This function updates the portIdLocal local copy of the sourcePortIdentity
* and writes this value into the TX PTP frame buffer templates. The fields
* that are written are:
* o sourcePortIdentity for all default PTP frames
* o Announce:: grandmasterIdentity
* o Announce:: TLV clockIdentity
*
* @param InstancePtr is a pointer to the XAvb instance to be worked on
* @param systemIdentity is the clockIdentity and portNumber for this endpoint
*
* @return None.
*
* @note Announce::TLV. By default the tlvType and length field are set
* up in the BRAM, assuming that N = 1.
*
****************************************************************************/
void XAvb_SetupSourcePortIdentity(XAvb * InstancePtr,
XAvb_PortIdentity systemIdentity)
{
u32 BufferWord;
u8 BufferEnable;
u32 DataBitEnable;
u32 ReadWord;
/* Set up our local copy of the sourcePortIdentity */
InstancePtr->portIdLocal = systemIdentity;
/** Write the sourcePortIdentity into the header for all TX PTP buffers
* except the empty default buffer AND write the GMID for TX announce AND
* Write the ClockIdentity into the TX Announce TLV */
BufferEnable = 0x7F;
/** (a) Write the upper 2 bytes of the ClockIdentityUpper
* - REM: Swap back the byte order into frame storage order */
DataBitEnable = 0xFFFF0000;
BufferWord = (systemIdentity.ClockIdentityUpper >> 16);
BufferWord = XAvb_ReorderWord(BufferWord);
XAvb_WriteToMultipleTxPtpFrames(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PKT_PORTID_UPPER_OFFSET,
BufferWord,
DataBitEnable,
BufferEnable);
ReadWord = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
XAVB_PTP_TX_PKT_ANNOUNCE_TLVLEN_PATHSEQ_START_OFFSET);
BufferWord = (ReadWord & 0x0000FFFF) | (BufferWord & 0xFFFF0000);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
XAVB_PTP_TX_PKT_ANNOUNCE_TLVLEN_PATHSEQ_START_OFFSET,
BufferWord);
/** (b) Write the lower 2 bytes of the ClockIdentityUpper and upper
* 2 bytes of the ClockIdentityLower.
* - REM: Swap back the byte order into frame storage order*/
BufferWord = ((systemIdentity.ClockIdentityUpper << 16) & 0xFFFF0000) | ((systemIdentity.ClockIdentityLower >> 16) & 0x0000FFFF);
BufferWord = XAvb_ReorderWord(BufferWord);
DataBitEnable = 0xFFFFFFFF;
XAvb_WriteToMultipleTxPtpFrames(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PKT_PORTID_MID_OFFSET,
BufferWord,
DataBitEnable,
BufferEnable);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
(XAVB_PTP_TX_PKT_ANNOUNCE_TLVLEN_PATHSEQ_START_OFFSET + 4),
BufferWord);
/** (c) Write the lower 2 bytes of the ClockIdentityLower and the portNumber
* - REM: Swap back the byte order into frame storage order*/
BufferWord = ((systemIdentity.ClockIdentityLower << 16) & 0xFFFF0000) | (systemIdentity.PortNumber & 0x0000FFFF);
BufferWord = XAvb_ReorderWord(BufferWord);
XAvb_WriteToMultipleTxPtpFrames(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_PKT_PORTID_LOWER_OFFSET,
BufferWord,
DataBitEnable,
BufferEnable);
BufferWord = (systemIdentity.ClockIdentityLower << 16) & 0xFFFF0000;
BufferWord = XAvb_ReorderWord(BufferWord);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
(XAVB_PTP_TX_PKT_ANNOUNCE_TLVLEN_PATHSEQ_START_OFFSET + 8),
BufferWord);
/** Write the grandmasterIdentity into the header for the TX Announce PTP buffer */
/** (a) Write 1 byte of GMID (Upper)*/
BufferWord = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
XAVB_PTP_TX_PKT_ANNOUNCE_QUAL_LOW_PRI2_GMID_HI_OFFSET);
BufferWord = (BufferWord & 0x00FFFFFF) | (systemIdentity.ClockIdentityUpper & 0xFF000000);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
XAVB_PTP_TX_PKT_ANNOUNCE_QUAL_LOW_PRI2_GMID_HI_OFFSET,
BufferWord);
/** (b) Write 3 bytes of GMID (Upper) and 1 byte of GMID (Lower)**/
BufferWord = (systemIdentity.ClockIdentityUpper << 8) | ((systemIdentity.ClockIdentityLower >> 24) & 0x000000FF);
BufferWord = XAvb_ReorderWord(BufferWord);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
XAVB_PTP_TX_PKT_ANNOUNCE_GMID_MID_OFFSET,
BufferWord);
/** (c) Write 3 bytes of GMID (Lower) */
BufferWord = XAvb_ReadPtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
XAVB_PTP_TX_PKT_ANNOUNCE_GMID_LOW_STEPSREMOVED_HI_OFFSET);
BufferWord = ((BufferWord >> 24) & 0x000000FF) | ((systemIdentity.ClockIdentityLower << 8) & 0xFFFFFF00);
BufferWord = XAvb_ReorderWord(BufferWord);
XAvb_WritePtpBuffer(InstancePtr->Config.BaseAddress,
XAVB_PTP_TX_ANNOUNCE_OFFSET,
XAVB_PTP_TX_PKT_ANNOUNCE_GMID_LOW_STEPSREMOVED_HI_OFFSET,
BufferWord);
#ifdef DEBUG_XAVB_LEVEL2
xil_printf("\r\n setupSourcePortIdentity: Writing systemIdentity to buffers %08x", BufferEnable);
xil_printf("\r\n setupSourcePortIdentity: ClockIdentityUpper --> 0x%08x, ClockIdentityLower --> 0x%08x, PortNumber --> 0x%08x",
InstancePtr->portIdLocal.ClockIdentityUpper,
InstancePtr->portIdLocal.ClockIdentityLower,
InstancePtr->portIdLocal.PortNumber);
#endif
}