diff options
Diffstat (limited to 'daemons/gptp/common/ptp_message.cpp')
| -rw-r--r-- | daemons/gptp/common/ptp_message.cpp | 2018 |
1 files changed, 0 insertions, 2018 deletions
diff --git a/daemons/gptp/common/ptp_message.cpp b/daemons/gptp/common/ptp_message.cpp deleted file mode 100644 index 8dc6775c..00000000 --- a/daemons/gptp/common/ptp_message.cpp +++ /dev/null @@ -1,2018 +0,0 @@ -/****************************************************************************** - - Copyright (c) 2009-2012, Intel Corporation - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - - 3. Neither the name of the Intel Corporation nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - POSSIBILITY OF SUCH DAMAGE. - -******************************************************************************/ - -#include <ieee1588.hpp> -#include <avbts_clock.hpp> -#include <avbts_message.hpp> -#include <ether_port.hpp> -#include <avbts_ostimer.hpp> -#include <ether_tstamper.hpp> - -#include <stdio.h> -#include <string.h> -#include <math.h> - -PTPMessageCommon::PTPMessageCommon( CommonPort *port ) -{ - // Fill in fields using port/clock dataset as a template - versionPTP = GPTP_VERSION; - versionNetwork = PTP_NETWORK_VERSION; - domainNumber = port->getClock()->getDomain(); - // Set flags as necessary - memset(flags, 0, PTP_FLAGS_LENGTH); - flags[PTP_PTPTIMESCALE_BYTE] |= (0x1 << PTP_PTPTIMESCALE_BIT); - correctionField = 0; - _gc = false; - sourcePortIdentity = new PortIdentity(); - - return; -} - -/* Determine whether the message was sent by given communication technology, - uuid, and port id fields */ -bool PTPMessageCommon::isSenderEqual(PortIdentity portIdentity) -{ - return portIdentity == *sourcePortIdentity; -} - -PTPMessageCommon *buildPTPMessage -( char *buf, int size, LinkLayerAddress *remote, - CommonPort *port ) -{ - OSTimer *timer = port->getTimerFactory()->createTimer(); - PTPMessageCommon *msg = NULL; - PTPMessageId messageId; - MessageType messageType; - unsigned char tspec_msg_t = 0; - unsigned char transportSpecific = 0; - - uint16_t sequenceId; - PortIdentity *sourcePortIdentity; - Timestamp timestamp(0, 0, 0); - unsigned counter_value = 0; - EtherPort *eport = NULL; - -#if PTP_DEBUG - { - int i; - GPTP_LOG_VERBOSE("Packet Dump:\n"); - for (i = 0; i < size; ++i) { - GPTP_LOG_VERBOSE("%hhx\t", buf[i]); - if (i % 8 == 7) - GPTP_LOG_VERBOSE("\n"); - } - if (i % 8 != 0) - GPTP_LOG_VERBOSE("\n"); - } -#endif - - memcpy(&tspec_msg_t, - buf + PTP_COMMON_HDR_TRANSSPEC_MSGTYPE(PTP_COMMON_HDR_OFFSET), - sizeof(tspec_msg_t)); - messageType = (MessageType) (tspec_msg_t & 0xF); - transportSpecific = (tspec_msg_t >> 4) & 0x0F; - - sourcePortIdentity = new PortIdentity - ((uint8_t *) - (buf + PTP_COMMON_HDR_SOURCE_CLOCK_ID - (PTP_COMMON_HDR_OFFSET)), - (uint16_t *) - (buf + PTP_COMMON_HDR_SOURCE_PORT_ID - (PTP_COMMON_HDR_OFFSET))); - - memcpy - (&(sequenceId), - buf + PTP_COMMON_HDR_SEQUENCE_ID(PTP_COMMON_HDR_OFFSET), - sizeof(sequenceId)); - sequenceId = PLAT_ntohs(sequenceId); - - GPTP_LOG_VERBOSE("Captured Sequence Id: %u", sequenceId); - messageId.setMessageType(messageType); - messageId.setSequenceId(sequenceId); - - - if (!(messageType >> 3)) { - int iter = 5; - long req = 4000; // = 1 ms - - eport = dynamic_cast <EtherPort *> ( port ); - if (eport == NULL) - { - GPTP_LOG_ERROR - ( "Received Event Message, but port type " - "doesn't support timestamping\n" ); - goto abort; - } - - int ts_good = - eport->getRxTimestamp - (sourcePortIdentity, messageId, timestamp, counter_value, false); - while (ts_good != GPTP_EC_SUCCESS && iter-- != 0) { - // Waits at least 1 time slice regardless of size of 'req' - timer->sleep(req); - if (ts_good != GPTP_EC_EAGAIN) - GPTP_LOG_ERROR( - "Error (RX) timestamping RX event packet (Retrying), error=%d", - ts_good ); - ts_good = - eport->getRxTimestamp(sourcePortIdentity, messageId, - timestamp, counter_value, - iter == 0); - req *= 2; - } - if (ts_good != GPTP_EC_SUCCESS) { - char err_msg[HWTIMESTAMPER_EXTENDED_MESSAGE_SIZE]; - port->getExtendedError(err_msg); - GPTP_LOG_ERROR - ("*** Received an event packet but cannot retrieve timestamp, discarding. messageType=%u,error=%d\n%s", - messageType, ts_good, msg); - //_exit(-1); - goto abort; - } - - else { - GPTP_LOG_VERBOSE("Timestamping event packet"); - } - - } - - if (1 != transportSpecific) { - GPTP_LOG_EXCEPTION("*** Received message with unsupported transportSpecific type=%d", transportSpecific); - goto abort; - } - - switch (messageType) { - case SYNC_MESSAGE: - - GPTP_LOG_DEBUG("*** Received Sync message" ); - GPTP_LOG_VERBOSE("Sync RX timestamp = %hu,%u,%u", timestamp.seconds_ms, timestamp.seconds_ls, timestamp.nanoseconds ); - - // Be sure buffer is the correction size - if (size < PTP_COMMON_HDR_LENGTH + PTP_SYNC_LENGTH) { - goto abort; - } - { - PTPMessageSync *sync_msg = new PTPMessageSync(); - sync_msg->messageType = messageType; - // Copy in v2 sync specific fields - memcpy(&(sync_msg->originTimestamp.seconds_ms), - buf + PTP_SYNC_SEC_MS(PTP_SYNC_OFFSET), - sizeof(sync_msg->originTimestamp.seconds_ms)); - memcpy(&(sync_msg->originTimestamp.seconds_ls), - buf + PTP_SYNC_SEC_LS(PTP_SYNC_OFFSET), - sizeof(sync_msg->originTimestamp.seconds_ls)); - memcpy(&(sync_msg->originTimestamp.nanoseconds), - buf + PTP_SYNC_NSEC(PTP_SYNC_OFFSET), - sizeof(sync_msg->originTimestamp.nanoseconds)); - msg = sync_msg; - } - break; - case FOLLOWUP_MESSAGE: - - GPTP_LOG_DEBUG("*** Received Follow Up message"); - - // Be sure buffer is the correction size - if (size < (int)(PTP_COMMON_HDR_LENGTH + PTP_FOLLOWUP_LENGTH + sizeof(FollowUpTLV))) { - goto abort; - } - { - PTPMessageFollowUp *followup_msg = - new PTPMessageFollowUp(); - followup_msg->messageType = messageType; - // Copy in v2 sync specific fields - memcpy(& - (followup_msg-> - preciseOriginTimestamp.seconds_ms), - buf + PTP_FOLLOWUP_SEC_MS(PTP_FOLLOWUP_OFFSET), - sizeof(followup_msg-> - preciseOriginTimestamp.seconds_ms)); - memcpy(& - (followup_msg-> - preciseOriginTimestamp.seconds_ls), - buf + PTP_FOLLOWUP_SEC_LS(PTP_FOLLOWUP_OFFSET), - sizeof(followup_msg-> - preciseOriginTimestamp.seconds_ls)); - memcpy(& - (followup_msg-> - preciseOriginTimestamp.nanoseconds), - buf + PTP_FOLLOWUP_NSEC(PTP_FOLLOWUP_OFFSET), - sizeof(followup_msg-> - preciseOriginTimestamp.nanoseconds)); - - followup_msg->preciseOriginTimestamp.seconds_ms = - PLAT_ntohs(followup_msg-> - preciseOriginTimestamp.seconds_ms); - followup_msg->preciseOriginTimestamp.seconds_ls = - PLAT_ntohl(followup_msg-> - preciseOriginTimestamp.seconds_ls); - followup_msg->preciseOriginTimestamp.nanoseconds = - PLAT_ntohl(followup_msg-> - preciseOriginTimestamp.nanoseconds); - - memcpy( &(followup_msg->tlv), - buf+PTP_FOLLOWUP_OFFSET+PTP_FOLLOWUP_LENGTH, - sizeof(followup_msg->tlv) ); - - msg = followup_msg; - } - - break; - case PATH_DELAY_REQ_MESSAGE: - - GPTP_LOG_DEBUG("*** Received PDelay Request message"); - - // Be sure buffer is the correction size - if (size < PTP_COMMON_HDR_LENGTH + PTP_PDELAY_REQ_LENGTH - && /* For Broadcom compatibility */ size != 46) { - goto abort; - } - { - PTPMessagePathDelayReq *pdelay_req_msg = - new PTPMessagePathDelayReq(); - pdelay_req_msg->messageType = messageType; - -#if 0 - /*TODO: Do we need the code below? Can we remove it?*/ - // The origin timestamp for PDelay Request packets has been eliminated since it is unused - // Copy in v2 PDelay Request specific fields - memcpy(&(pdelay_req_msg->originTimestamp.seconds_ms), - buf + - PTP_PDELAY_REQ_SEC_MS(PTP_PDELAY_REQ_OFFSET), - sizeof(pdelay_req_msg-> - originTimestamp.seconds_ms)); - memcpy(&(pdelay_req_msg->originTimestamp.seconds_ls), - buf + - PTP_PDELAY_REQ_SEC_LS(PTP_PDELAY_REQ_OFFSET), - sizeof(pdelay_req_msg-> - originTimestamp.seconds_ls)); - memcpy(&(pdelay_req_msg->originTimestamp.nanoseconds), - buf + PTP_PDELAY_REQ_NSEC(PTP_PDELAY_REQ_OFFSET), - sizeof(pdelay_req_msg-> - originTimestamp.nanoseconds)); - - pdelay_req_msg->originTimestamp.seconds_ms = - PLAT_ntohs(pdelay_req_msg-> - originTimestamp.seconds_ms); - pdelay_req_msg->originTimestamp.seconds_ls = - PLAT_ntohl(pdelay_req_msg-> - originTimestamp.seconds_ls); - pdelay_req_msg->originTimestamp.nanoseconds = - PLAT_ntohl(pdelay_req_msg-> - originTimestamp.nanoseconds); -#endif - - msg = pdelay_req_msg; - } - break; - case PATH_DELAY_RESP_MESSAGE: - - GPTP_LOG_DEBUG("*** Received PDelay Response message, Timestamp %u (sec) %u (ns), seqID %u", - timestamp.seconds_ls, timestamp.nanoseconds, - sequenceId); - - // Be sure buffer is the correction size - if (size < PTP_COMMON_HDR_LENGTH + PTP_PDELAY_RESP_LENGTH) { - goto abort; - } - { - PTPMessagePathDelayResp *pdelay_resp_msg = - new PTPMessagePathDelayResp(); - pdelay_resp_msg->messageType = messageType; - // Copy in v2 PDelay Response specific fields - pdelay_resp_msg->requestingPortIdentity = - new PortIdentity((uint8_t *) buf + - PTP_PDELAY_RESP_REQ_CLOCK_ID - (PTP_PDELAY_RESP_OFFSET), - (uint16_t *) (buf + - PTP_PDELAY_RESP_REQ_PORT_ID - (PTP_PDELAY_RESP_OFFSET))); - -#ifdef DEBUG - for (int n = 0; n < PTP_CLOCK_IDENTITY_LENGTH; ++n) { // MMM - GPTP_LOG_VERBOSE("%c", - pdelay_resp_msg-> - requestingPortIdentity.clockIdentity - [n]); - } -#endif - - memcpy(& (pdelay_resp_msg->requestReceiptTimestamp.seconds_ms), - buf + PTP_PDELAY_RESP_SEC_MS(PTP_PDELAY_RESP_OFFSET), - sizeof - (pdelay_resp_msg->requestReceiptTimestamp.seconds_ms)); - memcpy(& - (pdelay_resp_msg-> - requestReceiptTimestamp.seconds_ls), - buf + - PTP_PDELAY_RESP_SEC_LS(PTP_PDELAY_RESP_OFFSET), - sizeof(pdelay_resp_msg-> - requestReceiptTimestamp.seconds_ls)); - memcpy(& - (pdelay_resp_msg-> - requestReceiptTimestamp.nanoseconds), - buf + - PTP_PDELAY_RESP_NSEC(PTP_PDELAY_RESP_OFFSET), - sizeof(pdelay_resp_msg-> - requestReceiptTimestamp.nanoseconds)); - - pdelay_resp_msg->requestReceiptTimestamp.seconds_ms = - PLAT_ntohs(pdelay_resp_msg->requestReceiptTimestamp.seconds_ms); - pdelay_resp_msg->requestReceiptTimestamp.seconds_ls = - PLAT_ntohl(pdelay_resp_msg->requestReceiptTimestamp.seconds_ls); - pdelay_resp_msg->requestReceiptTimestamp.nanoseconds = - PLAT_ntohl(pdelay_resp_msg->requestReceiptTimestamp.nanoseconds); - - msg = pdelay_resp_msg; - } - break; - case PATH_DELAY_FOLLOWUP_MESSAGE: - - GPTP_LOG_DEBUG("*** Received PDelay Response FollowUp message"); - - // Be sure buffer is the correction size -// if( size < PTP_COMMON_HDR_LENGTH + PTP_PDELAY_FOLLOWUP_LENGTH ) { -// goto abort; -// } - { - PTPMessagePathDelayRespFollowUp *pdelay_resp_fwup_msg = - new PTPMessagePathDelayRespFollowUp(); - pdelay_resp_fwup_msg->messageType = messageType; - // Copy in v2 PDelay Response specific fields - pdelay_resp_fwup_msg->requestingPortIdentity = - new PortIdentity((uint8_t *) buf + - PTP_PDELAY_FOLLOWUP_REQ_CLOCK_ID - (PTP_PDELAY_RESP_OFFSET), - (uint16_t *) (buf + - PTP_PDELAY_FOLLOWUP_REQ_PORT_ID - (PTP_PDELAY_FOLLOWUP_OFFSET))); - - memcpy(& - (pdelay_resp_fwup_msg-> - responseOriginTimestamp.seconds_ms), - buf + - PTP_PDELAY_FOLLOWUP_SEC_MS - (PTP_PDELAY_FOLLOWUP_OFFSET), - sizeof - (pdelay_resp_fwup_msg->responseOriginTimestamp. - seconds_ms)); - memcpy(& - (pdelay_resp_fwup_msg-> - responseOriginTimestamp.seconds_ls), - buf + - PTP_PDELAY_FOLLOWUP_SEC_LS - (PTP_PDELAY_FOLLOWUP_OFFSET), - sizeof - (pdelay_resp_fwup_msg->responseOriginTimestamp. - seconds_ls)); - memcpy(& - (pdelay_resp_fwup_msg-> - responseOriginTimestamp.nanoseconds), - buf + - PTP_PDELAY_FOLLOWUP_NSEC - (PTP_PDELAY_FOLLOWUP_OFFSET), - sizeof - (pdelay_resp_fwup_msg->responseOriginTimestamp. - nanoseconds)); - - pdelay_resp_fwup_msg-> - responseOriginTimestamp.seconds_ms = - PLAT_ntohs - (pdelay_resp_fwup_msg->responseOriginTimestamp. - seconds_ms); - pdelay_resp_fwup_msg-> - responseOriginTimestamp.seconds_ls = - PLAT_ntohl - (pdelay_resp_fwup_msg->responseOriginTimestamp. - seconds_ls); - pdelay_resp_fwup_msg-> - responseOriginTimestamp.nanoseconds = - PLAT_ntohl - (pdelay_resp_fwup_msg->responseOriginTimestamp. - nanoseconds); - - msg = pdelay_resp_fwup_msg; - } - break; - case ANNOUNCE_MESSAGE: - - GPTP_LOG_VERBOSE("*** Received Announce message"); - - { - PTPMessageAnnounce *annc = new PTPMessageAnnounce(); - annc->messageType = messageType; - int tlv_length = size - PTP_COMMON_HDR_LENGTH + PTP_ANNOUNCE_LENGTH; - - memcpy(&(annc->currentUtcOffset), - buf + - PTP_ANNOUNCE_CURRENT_UTC_OFFSET - (PTP_ANNOUNCE_OFFSET), - sizeof(annc->currentUtcOffset)); - annc->currentUtcOffset = - PLAT_ntohs(annc->currentUtcOffset); - memcpy(&(annc->grandmasterPriority1), - buf + - PTP_ANNOUNCE_GRANDMASTER_PRIORITY1 - (PTP_ANNOUNCE_OFFSET), - sizeof(annc->grandmasterPriority1)); - memcpy( annc->grandmasterClockQuality, - buf+ - PTP_ANNOUNCE_GRANDMASTER_CLOCK_QUALITY - (PTP_ANNOUNCE_OFFSET), - sizeof( *annc->grandmasterClockQuality )); - annc-> - grandmasterClockQuality->offsetScaledLogVariance = - PLAT_ntohs - ( annc->grandmasterClockQuality-> - offsetScaledLogVariance ); - memcpy(&(annc->grandmasterPriority2), - buf + - PTP_ANNOUNCE_GRANDMASTER_PRIORITY2 - (PTP_ANNOUNCE_OFFSET), - sizeof(annc->grandmasterPriority2)); - memcpy(&(annc->grandmasterIdentity), - buf + - PTP_ANNOUNCE_GRANDMASTER_IDENTITY - (PTP_ANNOUNCE_OFFSET), - PTP_CLOCK_IDENTITY_LENGTH); - memcpy(&(annc->stepsRemoved), - buf + - PTP_ANNOUNCE_STEPS_REMOVED(PTP_ANNOUNCE_OFFSET), - sizeof(annc->stepsRemoved)); - annc->stepsRemoved = PLAT_ntohs(annc->stepsRemoved); - memcpy(&(annc->timeSource), - buf + - PTP_ANNOUNCE_TIME_SOURCE(PTP_ANNOUNCE_OFFSET), - sizeof(annc->timeSource)); - - // Parse TLV if it exists - buf += PTP_COMMON_HDR_LENGTH + PTP_ANNOUNCE_LENGTH; - if( tlv_length > (int) (2*sizeof(uint16_t)) && PLAT_ntohs(*((uint16_t *)buf)) == PATH_TRACE_TLV_TYPE) { - buf += sizeof(uint16_t); - tlv_length -= sizeof(uint16_t); - annc->tlv.parseClockIdentity((uint8_t *)buf, tlv_length); - } - - msg = annc; - } - break; - - case SIGNALLING_MESSAGE: - { - PTPMessageSignalling *signallingMsg = new PTPMessageSignalling(); - signallingMsg->messageType = messageType; - - memcpy(&(signallingMsg->targetPortIdentify), - buf + PTP_SIGNALLING_TARGET_PORT_IDENTITY(PTP_SIGNALLING_OFFSET), - sizeof(signallingMsg->targetPortIdentify)); - - memcpy( &(signallingMsg->tlv), buf + PTP_SIGNALLING_OFFSET + PTP_SIGNALLING_LENGTH, sizeof(signallingMsg->tlv) ); - - msg = signallingMsg; - } - break; - - default: - - GPTP_LOG_EXCEPTION("Received unsupported message type, %d", - (int)messageType); - port->incCounter_ieee8021AsPortStatRxPTPPacketDiscard(); - - goto abort; - } - - msg->_gc = false; - - // Copy in common header fields - memcpy(&(msg->versionPTP), - buf + PTP_COMMON_HDR_PTP_VERSION(PTP_COMMON_HDR_OFFSET), - sizeof(msg->versionPTP)); - memcpy(&(msg->messageLength), - buf + PTP_COMMON_HDR_MSG_LENGTH(PTP_COMMON_HDR_OFFSET), - sizeof(msg->messageLength)); - msg->messageLength = PLAT_ntohs(msg->messageLength); - memcpy(&(msg->domainNumber), - buf + PTP_COMMON_HDR_DOMAIN_NUMBER(PTP_COMMON_HDR_OFFSET), - sizeof(msg->domainNumber)); - memcpy(&(msg->flags), buf + PTP_COMMON_HDR_FLAGS(PTP_COMMON_HDR_OFFSET), - PTP_FLAGS_LENGTH); - memcpy(&(msg->correctionField), - buf + PTP_COMMON_HDR_CORRECTION(PTP_COMMON_HDR_OFFSET), - sizeof(msg->correctionField)); - msg->correctionField = PLAT_ntohll(msg->correctionField); - msg->sourcePortIdentity = sourcePortIdentity; - msg->sequenceId = sequenceId; - memcpy(&(msg->control), - buf + PTP_COMMON_HDR_CONTROL(PTP_COMMON_HDR_OFFSET), - sizeof(msg->control)); - memcpy(&(msg->logMeanMessageInterval), - buf + PTP_COMMON_HDR_LOG_MSG_INTRVL(PTP_COMMON_HDR_OFFSET), - sizeof(msg->logMeanMessageInterval)); - - if( eport != NULL ) - eport->addSockAddrMap( msg->sourcePortIdentity, remote ); - - msg->_timestamp = timestamp; - msg->_timestamp_counter_value = counter_value; - - delete timer; - - return msg; - -abort: - delete sourcePortIdentity; - delete timer; - - return NULL; -} - -bool PTPMessageCommon::getTxTimestamp( EtherPort *port, uint32_t link_speed ) -{ - OSTimer *timer = port->getTimerFactory()->createTimer(); - int ts_good; - Timestamp tx_timestamp; - uint32_t unused; - unsigned req = TX_TIMEOUT_BASE; - int iter = TX_TIMEOUT_ITER; - - ts_good = port->getTxTimestamp - ( this, tx_timestamp, unused, false ); - while( ts_good != GPTP_EC_SUCCESS && iter-- != 0 ) - { - timer->sleep(req); - if (ts_good != GPTP_EC_EAGAIN && iter < 1) - GPTP_LOG_ERROR( - "Error (TX) timestamping PDelay request " - "(Retrying-%d), error=%d", iter, ts_good); - ts_good = port->getTxTimestamp - ( this, tx_timestamp, unused , iter == 0 ); - req *= 2; - } - - if( ts_good == GPTP_EC_SUCCESS ) - { - Timestamp phy_compensation = port->getTxPhyDelay( link_speed ); - GPTP_LOG_DEBUG( "TX PHY compensation: %s sec", - phy_compensation.toString().c_str() ); - phy_compensation._version = tx_timestamp._version; - _timestamp = tx_timestamp + phy_compensation; - } else - { - char msg[HWTIMESTAMPER_EXTENDED_MESSAGE_SIZE]; - port->getExtendedError(msg); - GPTP_LOG_ERROR( - "Error (TX) timestamping PDelay request, error=%d\t%s", - ts_good, msg); - _timestamp = INVALID_TIMESTAMP; - } - - delete timer; - return ts_good == GPTP_EC_SUCCESS; -} - -void PTPMessageCommon::processMessage( CommonPort *port ) -{ - _gc = true; - return; -} - -void PTPMessageCommon::buildCommonHeader(uint8_t * buf) -{ - unsigned char tspec_msg_t; - /*TODO: Message type assumes value sbetween 0x0 and 0xD (its an enumeration). - * So I am not sure why we are adding 0x10 to it - */ - tspec_msg_t = messageType | 0x10; - long long correctionField_BE = PLAT_htonll(correctionField); - uint16_t messageLength_NO = PLAT_htons(messageLength); - - memcpy(buf + PTP_COMMON_HDR_TRANSSPEC_MSGTYPE(PTP_COMMON_HDR_OFFSET), - &tspec_msg_t, sizeof(tspec_msg_t)); - memcpy(buf + PTP_COMMON_HDR_PTP_VERSION(PTP_COMMON_HDR_OFFSET), - &versionPTP, sizeof(versionPTP)); - memcpy(buf + PTP_COMMON_HDR_MSG_LENGTH(PTP_COMMON_HDR_OFFSET), - &messageLength_NO, sizeof(messageLength_NO)); - memcpy(buf + PTP_COMMON_HDR_DOMAIN_NUMBER(PTP_COMMON_HDR_OFFSET), - &domainNumber, sizeof(domainNumber)); - memcpy(buf + PTP_COMMON_HDR_FLAGS(PTP_COMMON_HDR_OFFSET), &flags, - PTP_FLAGS_LENGTH); - memcpy(buf + PTP_COMMON_HDR_CORRECTION(PTP_COMMON_HDR_OFFSET), - &correctionField_BE, sizeof(correctionField)); - - sourcePortIdentity->getClockIdentityString - ((uint8_t *) buf+ - PTP_COMMON_HDR_SOURCE_CLOCK_ID(PTP_COMMON_HDR_OFFSET)); - sourcePortIdentity->getPortNumberNO - ((uint16_t *) (buf + PTP_COMMON_HDR_SOURCE_PORT_ID - (PTP_COMMON_HDR_OFFSET))); - - GPTP_LOG_VERBOSE("Sending Sequence Id: %u", sequenceId); - sequenceId = PLAT_htons(sequenceId); - memcpy(buf + PTP_COMMON_HDR_SEQUENCE_ID(PTP_COMMON_HDR_OFFSET), - &sequenceId, sizeof(sequenceId)); - sequenceId = PLAT_ntohs(sequenceId); - memcpy(buf + PTP_COMMON_HDR_CONTROL(PTP_COMMON_HDR_OFFSET), &control, - sizeof(control)); - memcpy(buf + PTP_COMMON_HDR_LOG_MSG_INTRVL(PTP_COMMON_HDR_OFFSET), - &logMeanMessageInterval, sizeof(logMeanMessageInterval)); - - return; -} - -void PTPMessageCommon::getPortIdentity(PortIdentity * identity) -{ - *identity = *sourcePortIdentity; -} - -void PTPMessageCommon::setPortIdentity(PortIdentity * identity) -{ - *sourcePortIdentity = *identity; -} - -PTPMessageCommon::~PTPMessageCommon(void) -{ - delete sourcePortIdentity; - return; -} - -PTPMessageAnnounce::PTPMessageAnnounce(void) -{ - grandmasterClockQuality = new ClockQuality(); -} - -PTPMessageAnnounce::~PTPMessageAnnounce(void) -{ - delete grandmasterClockQuality; -} - -bool PTPMessageAnnounce::isBetterThan(PTPMessageAnnounce * msg) -{ - unsigned char this1[14]; - unsigned char that1[14]; - uint16_t tmp; - - this1[0] = grandmasterPriority1; - that1[0] = msg->getGrandmasterPriority1(); - - this1[1] = grandmasterClockQuality->cq_class; - that1[1] = msg->getGrandmasterClockQuality()->cq_class; - - this1[2] = grandmasterClockQuality->clockAccuracy; - that1[2] = msg->getGrandmasterClockQuality()->clockAccuracy; - - tmp = grandmasterClockQuality->offsetScaledLogVariance; - tmp = PLAT_htons(tmp); - memcpy(this1 + 3, &tmp, sizeof(tmp)); - tmp = msg->getGrandmasterClockQuality()->offsetScaledLogVariance; - tmp = PLAT_htons(tmp); - memcpy(that1 + 3, &tmp, sizeof(tmp)); - - this1[5] = grandmasterPriority2; - that1[5] = msg->getGrandmasterPriority2(); - - this->getGrandmasterIdentity((char *)this1 + 6); - msg->getGrandmasterIdentity((char *)that1 + 6); - -#if 0 - GPTP_LOG_VERBOSE("Us: "); - for (int i = 0; i < 14; ++i) - GPTP_LOG_VERBOSE("%hhx", this1[i]); - GPTP_LOG_VERBOSE("\n"); - GPTP_LOG_VERBOSE("Them: "); - for (int i = 0; i < 14; ++i) - GPTP_LOG_VERBOSE("%hhx", that1[i]); - GPTP_LOG_VERBOSE("\n"); -#endif - - return (memcmp(this1, that1, 14) < 0) ? true : false; -} - - -PTPMessageSync::PTPMessageSync() { -} - -PTPMessageSync::~PTPMessageSync() { -} - -PTPMessageSync::PTPMessageSync( EtherPort *port ) : - PTPMessageCommon( port ) -{ - messageType = SYNC_MESSAGE; // This is an event message - sequenceId = port->getNextSyncSequenceId(); - control = SYNC; - - flags[PTP_ASSIST_BYTE] |= (0x1 << PTP_ASSIST_BIT); - - originTimestamp = port->getClock()->getTime(); - - logMeanMessageInterval = port->getSyncInterval(); - return; -} - -bool PTPMessageSync::sendPort -( EtherPort *port, PortIdentity *destIdentity ) -{ - uint8_t buf_t[256]; - uint8_t *buf_ptr = buf_t + port->getPayloadOffset(); - unsigned char tspec_msg_t = 0x0; - Timestamp originTimestamp_BE; - uint32_t link_speed; - - memset(buf_t, 0, 256); - // Create packet in buf - // Copy in common header - messageLength = PTP_COMMON_HDR_LENGTH + PTP_SYNC_LENGTH; - tspec_msg_t |= messageType & 0xF; - buildCommonHeader(buf_ptr); - // Get timestamp - originTimestamp = port->getClock()->getTime(); - originTimestamp_BE.seconds_ms = PLAT_htons(originTimestamp.seconds_ms); - originTimestamp_BE.seconds_ls = PLAT_htonl(originTimestamp.seconds_ls); - originTimestamp_BE.nanoseconds = - PLAT_htonl(originTimestamp.nanoseconds); - // Copy in v2 sync specific fields - memcpy(buf_ptr + PTP_SYNC_SEC_MS(PTP_SYNC_OFFSET), - &(originTimestamp_BE.seconds_ms), - sizeof(originTimestamp.seconds_ms)); - memcpy(buf_ptr + PTP_SYNC_SEC_LS(PTP_SYNC_OFFSET), - &(originTimestamp_BE.seconds_ls), - sizeof(originTimestamp.seconds_ls)); - memcpy(buf_ptr + PTP_SYNC_NSEC(PTP_SYNC_OFFSET), - &(originTimestamp_BE.nanoseconds), - sizeof(originTimestamp.nanoseconds)); - - port->sendEventPort - ( PTP_ETHERTYPE, buf_t, messageLength, MCAST_OTHER, - destIdentity, &link_speed ); - port->incCounter_ieee8021AsPortStatTxSyncCount(); - - return getTxTimestamp( port, link_speed ); -} - -PTPMessageAnnounce::PTPMessageAnnounce( CommonPort *port ) : - PTPMessageCommon( port ) -{ - messageType = ANNOUNCE_MESSAGE; // This is an event message - sequenceId = port->getNextAnnounceSequenceId(); - ClockIdentity id; - control = MESSAGE_OTHER; - ClockIdentity clock_identity; - - id = port->getClock()->getClockIdentity(); - tlv.appendClockIdentity(&id); - - currentUtcOffset = port->getClock()->getCurrentUtcOffset(); - grandmasterPriority1 = port->getClock()->getPriority1(); - grandmasterPriority2 = port->getClock()->getPriority2(); - grandmasterClockQuality = new ClockQuality(); - *grandmasterClockQuality = port->getClock()->getClockQuality(); - stepsRemoved = 0; - timeSource = port->getClock()->getTimeSource(); - clock_identity = port->getClock()->getGrandmasterClockIdentity(); - clock_identity.getIdentityString(grandmasterIdentity); - - logMeanMessageInterval = port->getAnnounceInterval(); - return; -} - -bool PTPMessageAnnounce::sendPort -( CommonPort *port, PortIdentity *destIdentity ) -{ - uint8_t buf_t[256]; - uint8_t *buf_ptr = buf_t + port->getPayloadOffset(); - unsigned char tspec_msg_t = 0x0; - - uint16_t currentUtcOffset_l = PLAT_htons(currentUtcOffset); - uint16_t stepsRemoved_l = PLAT_htons(stepsRemoved); - ClockQuality clockQuality_l = *grandmasterClockQuality; - clockQuality_l.offsetScaledLogVariance = - PLAT_htons(clockQuality_l.offsetScaledLogVariance); - - memset(buf_t, 0, 256); - // Create packet in buf - // Copy in common header - messageLength = - PTP_COMMON_HDR_LENGTH + PTP_ANNOUNCE_LENGTH + tlv.length(); - tspec_msg_t |= messageType & 0xF; - buildCommonHeader(buf_ptr); - memcpy(buf_ptr + PTP_ANNOUNCE_CURRENT_UTC_OFFSET(PTP_ANNOUNCE_OFFSET), - ¤tUtcOffset_l, sizeof(currentUtcOffset)); - memcpy(buf_ptr + - PTP_ANNOUNCE_GRANDMASTER_PRIORITY1(PTP_ANNOUNCE_OFFSET), - &grandmasterPriority1, sizeof(grandmasterPriority1)); - memcpy(buf_ptr + - PTP_ANNOUNCE_GRANDMASTER_CLOCK_QUALITY(PTP_ANNOUNCE_OFFSET), - &clockQuality_l, sizeof(clockQuality_l)); - memcpy(buf_ptr + - PTP_ANNOUNCE_GRANDMASTER_PRIORITY2(PTP_ANNOUNCE_OFFSET), - &grandmasterPriority2, sizeof(grandmasterPriority2)); - memcpy( buf_ptr+ - PTP_ANNOUNCE_GRANDMASTER_IDENTITY(PTP_ANNOUNCE_OFFSET), - grandmasterIdentity, PTP_CLOCK_IDENTITY_LENGTH ); - memcpy(buf_ptr + PTP_ANNOUNCE_STEPS_REMOVED(PTP_ANNOUNCE_OFFSET), - &stepsRemoved_l, sizeof(stepsRemoved)); - memcpy(buf_ptr + PTP_ANNOUNCE_TIME_SOURCE(PTP_ANNOUNCE_OFFSET), - &timeSource, sizeof(timeSource)); - tlv.toByteString(buf_ptr + PTP_COMMON_HDR_LENGTH + PTP_ANNOUNCE_LENGTH); - - port->sendGeneralPort(PTP_ETHERTYPE, buf_t, messageLength, MCAST_OTHER, destIdentity); - port->incCounter_ieee8021AsPortStatTxAnnounce(); - - return true; -} - -void PTPMessageAnnounce::processMessage( CommonPort *port ) -{ - ClockIdentity my_clock_identity; - - port->incCounter_ieee8021AsPortStatRxAnnounce(); - - // Delete announce receipt timeout - port->getClock()->deleteEventTimerLocked - (port, ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES); - - if( stepsRemoved >= 255 ) goto bail; - - // Reject Announce message from myself - my_clock_identity = port->getClock()->getClockIdentity(); - if( sourcePortIdentity->getClockIdentity() == my_clock_identity ) { - goto bail; - } - - if(tlv.has(&my_clock_identity)) { - goto bail; - } - - // Add message to the list - port->setQualifiedAnnounce( this ); - - port->getClock()->addEventTimerLocked(port, STATE_CHANGE_EVENT, 16000000); - bail: - port->getClock()->addEventTimerLocked - (port, ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES, - (unsigned long long) - (ANNOUNCE_RECEIPT_TIMEOUT_MULTIPLIER * - (pow - ((double)2, - port->getAnnounceInterval()) * - 1000000000.0))); -} - -void PTPMessageSync::processMessage( CommonPort *port ) -{ - EtherPort *eport = dynamic_cast <EtherPort *> (port); - PTPMessageSync *old_sync; - - if (eport == NULL) - { - GPTP_LOG_ERROR( "Discarding sync message on wrong port type" ); - _gc = true; - goto done; - } - - if (port->getPortState() == PTP_DISABLED ) { - // Do nothing Sync messages should be ignored in this state - return; - } - if (port->getPortState() == PTP_FAULTY) { - // According to spec recovery is implementation specific - eport->recoverPort(); - return; - } - - port->incCounter_ieee8021AsPortStatRxSyncCount(); - -#if CHECK_ASSIST_BIT - if( flags[PTP_ASSIST_BYTE] & (0x1<<PTP_ASSIST_BIT)) { -#endif - old_sync = eport->getLastSync(); - - if (old_sync != NULL) { - delete old_sync; - } - eport->setLastSync(this); - _gc = false; - goto done; -#if CHECK_ASSIST_BIT - } else { - GPTP_LOG_ERROR("PTP assist flag is not set, discarding invalid sync"); - _gc = true; - goto done; - } -#endif - - done: - return; -} - -PTPMessageFollowUp::PTPMessageFollowUp( CommonPort *port ) : - PTPMessageCommon( port ) -{ - messageType = FOLLOWUP_MESSAGE; /* This is an event message */ - control = FOLLOWUP; - - logMeanMessageInterval = port->getSyncInterval(); - - return; -} - -size_t PTPMessageFollowUp::buildMessage( CommonPort *port, uint8_t *buf_ptr ) -{ - /* Create packet in buf - Copy in common header */ - messageLength = - PTP_COMMON_HDR_LENGTH + PTP_FOLLOWUP_LENGTH + sizeof(tlv); - unsigned char tspec_msg_t = 0; - Timestamp preciseOriginTimestamp_BE; - - tspec_msg_t |= messageType & 0xF; - buildCommonHeader(buf_ptr); - preciseOriginTimestamp_BE.seconds_ms = - PLAT_htons(preciseOriginTimestamp.seconds_ms); - preciseOriginTimestamp_BE.seconds_ls = - PLAT_htonl(preciseOriginTimestamp.seconds_ls); - preciseOriginTimestamp_BE.nanoseconds = - PLAT_htonl(preciseOriginTimestamp.nanoseconds); - /* Copy in v2 sync specific fields */ - memcpy(buf_ptr + PTP_FOLLOWUP_SEC_MS(PTP_FOLLOWUP_OFFSET), - &(preciseOriginTimestamp_BE.seconds_ms), - sizeof(preciseOriginTimestamp.seconds_ms)); - memcpy(buf_ptr + PTP_FOLLOWUP_SEC_LS(PTP_FOLLOWUP_OFFSET), - &(preciseOriginTimestamp_BE.seconds_ls), - sizeof(preciseOriginTimestamp.seconds_ls)); - memcpy(buf_ptr + PTP_FOLLOWUP_NSEC(PTP_FOLLOWUP_OFFSET), - &(preciseOriginTimestamp_BE.nanoseconds), - sizeof(preciseOriginTimestamp.nanoseconds)); - - /*Change time base indicator to Network Order before sending it*/ - uint16_t tbi_NO = PLAT_htonl(tlv.getGMTimeBaseIndicator()); - tlv.setGMTimeBaseIndicator(tbi_NO); - tlv.toByteString(buf_ptr + PTP_COMMON_HDR_LENGTH + - PTP_FOLLOWUP_LENGTH); - - port->incCounter_ieee8021AsPortStatTxFollowUpCount(); - - return PTP_COMMON_HDR_LENGTH + PTP_FOLLOWUP_LENGTH + sizeof(tlv); -} - -bool PTPMessageFollowUp::sendPort -( EtherPort *port, PortIdentity *destIdentity ) -{ - uint8_t buf_t[256]; - uint8_t *buf_ptr = buf_t + port->getPayloadOffset(); - memset(buf_t, 0, 256); - /* Create packet in buf - Copy in common header */ - buildMessage(port, buf_ptr); - - GPTP_LOG_VERBOSE( "Follow-Up Time: %u seconds(hi)", - preciseOriginTimestamp.seconds_ms); - GPTP_LOG_VERBOSE( "Follow-Up Time: %u seconds", - preciseOriginTimestamp.seconds_ls); - GPTP_LOG_VERBOSE( "FW-UP Time: %u nanoseconds", - preciseOriginTimestamp.nanoseconds); - GPTP_LOG_VERBOSE( "FW-UP Time: %x seconds", - preciseOriginTimestamp.seconds_ls); - GPTP_LOG_VERBOSE( "FW-UP Time: %x nanoseconds", - preciseOriginTimestamp.nanoseconds); -#ifdef DEBUG - GPTP_LOG_VERBOSE("Follow-up Dump:"); - for (int i = 0; i < messageLength; ++i) { - GPTP_LOG_VERBOSE("%d:%02x ", i, (unsigned char)buf_t[i]); - } -#endif - - port->sendGeneralPort( PTP_ETHERTYPE, buf_t, messageLength, - MCAST_OTHER, destIdentity ); - - return true; -} - -void PTPMessageFollowUp::processMessage -( CommonPort *port, Timestamp sync_arrival ) -{ - uint64_t delay; - Timestamp system_time(0, 0, 0); - Timestamp device_time(0, 0, 0); - - signed long long local_system_offset; - signed long long scalar_offset; - - FrequencyRatio local_clock_adjustment; - FrequencyRatio local_system_freq_offset; - FrequencyRatio master_local_freq_offset; - int64_t correction; - int32_t scaledLastGmFreqChange = 0; - scaledNs scaledLastGmPhaseChange; - - port->incCounter_ieee8021AsPortStatRxFollowUpCount(); - - if (!port->getLinkDelay(&delay)) - { - GPTP_LOG_ERROR( "Received Follow up but " - "there is no valid link delay" ); - goto done; - } - - master_local_freq_offset = tlv.getRateOffset(); - master_local_freq_offset /= 1ULL << 41; - master_local_freq_offset += 1.0; - master_local_freq_offset /= port->getPeerRateOffset(); - - correctionField /= 1 << 16; - correction = (int64_t) - ((delay * master_local_freq_offset) + correctionField); - - if (correction > 0) - TIMESTAMP_ADD_NS(preciseOriginTimestamp, correction); - else TIMESTAMP_SUB_NS(preciseOriginTimestamp, -correction); - - local_clock_adjustment = - port->getClock()-> - calcMasterLocalClockRateDifference - (preciseOriginTimestamp, sync_arrival); - - if( local_clock_adjustment == NEGATIVE_TIME_JUMP ) - { - GPTP_LOG_VERBOSE - ( "Received Follow Up but preciseOrigintimestamp " - "indicates negative time jump" ); - goto done; - } - - scalar_offset = TIMESTAMP_TO_NS( sync_arrival ); - scalar_offset -= TIMESTAMP_TO_NS( preciseOriginTimestamp ); - - GPTP_LOG_VERBOSE( "Followup Correction Field: %lld, Link Delay: %lu", - correctionField, delay ); - GPTP_LOG_VERBOSE( "FollowUp Scalar = %lld", - scalar_offset ); - - - /* Otherwise synchronize clock with approximate Sync time */ - uint32_t local_clock, nominal_clock_rate; - uint32_t device_sync_time_offset; - - port->getDeviceTime(system_time, device_time, local_clock, - nominal_clock_rate); - GPTP_LOG_VERBOSE( "Device Time = %llu,System Time = %llu", - TIMESTAMP_TO_NS( device_time ), - TIMESTAMP_TO_NS( system_time )); - - /* Adjust local_clock to correspond to sync_arrival */ - device_sync_time_offset = (uint32_t) - ( TIMESTAMP_TO_NS( device_time ) - - TIMESTAMP_TO_NS( sync_arrival )); - - GPTP_LOG_VERBOSE - ( "ptp_message::FollowUp::processMessage System time: %u,%u " - "Device Time: %u,%u", - system_time.seconds_ls, system_time.nanoseconds, - device_time.seconds_ls, device_time.nanoseconds); - - /*Update information on local status structure.*/ - scaledLastGmFreqChange = (int32_t) - ((1.0 / local_clock_adjustment - 1.0) * (1ULL << 41)); - scaledLastGmPhaseChange.setLSB(tlv.getRateOffset( )); - port->getClock()->getFUPStatus()->setScaledLastGmFreqChange - ( scaledLastGmFreqChange ); - port->getClock()->getFUPStatus()->setScaledLastGmPhaseChange - ( scaledLastGmPhaseChange ); - - if( port->getPortState() == PTP_SLAVE ) - { - /* - * The sync_count counts the number of sync messages received - * that influence the time on the device. Since adjustments are - * only made in the PTP_SLAVE state, increment it here - */ - port->incSyncCount(); - - /* - * Do not call calcLocalSystemClockRateDifference it updates - * state global to the clock object and if we are master then - * the network is transitioning to us not being master but - * the master process is still running locally - */ - local_system_freq_offset = port->getClock() - ->calcLocalSystemClockRateDifference - ( device_time, system_time ); - TIMESTAMP_SUB_NS - (system_time, (uint64_t) - (((FrequencyRatio)device_sync_time_offset) / - local_system_freq_offset)); - local_system_offset = - TIMESTAMP_TO_NS( system_time ) - - TIMESTAMP_TO_NS( sync_arrival ); - - port->getClock()->setMasterOffset - ( port, scalar_offset, sync_arrival, local_clock_adjustment, - local_system_offset, system_time, local_system_freq_offset, - port->getSyncCount(), port->getPdelayCount(), - port->getPortState(), port->getAsCapable( )); - - port->syncDone(); - // Restart the SYNC_RECEIPT timer - port->startSyncReceiptTimer((unsigned long long) - (SYNC_RECEIPT_TIMEOUT_MULTIPLIER * - ((double)pow((double)2, port->getSyncInterval()) * - 1000000000.0))); - } - - uint16_t lastGmTimeBaseIndicator; - lastGmTimeBaseIndicator = port->getLastGmTimeBaseIndicator(); - if (( lastGmTimeBaseIndicator > 0 ) && - ( tlv.getGmTimeBaseIndicator( ) != lastGmTimeBaseIndicator )) - { - GPTP_LOG_EXCEPTION( "Sync discontinuity" ); - } - port->setLastGmTimeBaseIndicator( tlv.getGmTimeBaseIndicator( )); - -done: - _gc = true; - - return; -} - -void PTPMessageFollowUp::processMessage( CommonPort *port ) -{ - Timestamp sync_arrival; - EtherPort *eport = dynamic_cast <EtherPort *> (port); - if (eport == NULL) - { - GPTP_LOG_ERROR - ( "Discarding followup message on wrong port type" ); - return; - } - - GPTP_LOG_DEBUG("Processing a follow-up message"); - - // Expire any SYNC_RECEIPT timers that exist - port->stopSyncReceiptTimer(); - - if (port->getPortState() == PTP_DISABLED ) { - // Do nothing Sync messages should be ignored when in this state - return; - } - if (port->getPortState() == PTP_FAULTY) { - // According to spec recovery is implementation specific - eport->recoverPort(); - return; - } - - PTPMessageSync *sync = eport->getLastSync(); - { - PortIdentity sync_id; - if( sync == NULL ) - { - GPTP_LOG_ERROR("Received Follow Up but there is no " - "sync message"); - return; - } - sync->getPortIdentity(&sync_id); - - if( sync->getSequenceId() != sequenceId || - sync_id != *sourcePortIdentity ) - { - unsigned int cnt = 0; - - if( !port->incWrongSeqIDCounter( &cnt )) - { - port->becomeMaster( true ); - port->setWrongSeqIDCounter(0); - } - GPTP_LOG_ERROR - ( "Received Follow Up %d times but cannot " - "find corresponding Sync", cnt ); - goto done; - } - } - - if( sync->getTimestamp()._version != port->getTimestampVersion( )) - { - GPTP_LOG_ERROR( "Received Follow Up but timestamp version " - "indicates Sync is out of date" ); - goto done; - } - - sync_arrival = sync->getTimestamp(); - - processMessage(port, sync_arrival); - -done: - eport->setLastSync(NULL); - delete sync; -} - -PTPMessagePathDelayReq::PTPMessagePathDelayReq -( EtherPort *port ) : PTPMessageCommon( port ) -{ - logMeanMessageInterval = 0; - control = MESSAGE_OTHER; - messageType = PATH_DELAY_REQ_MESSAGE; - sequenceId = port->getNextPDelaySequenceId(); - return; -} - -void PTPMessagePathDelayReq::processMessage( CommonPort *port ) -{ - OSTimer *timer = port->getTimerFactory()->createTimer(); - PortIdentity resp_fwup_id; - PortIdentity requestingPortIdentity_p; - PTPMessagePathDelayResp *resp; - PortIdentity resp_id; - PTPMessagePathDelayRespFollowUp *resp_fwup; - - EtherPort *eport = dynamic_cast <EtherPort *> (port); - if (eport == NULL) - { - GPTP_LOG_ERROR( "Received Pdelay Request on wrong port type" ); - goto done; - } - - if (port->getPortState() == PTP_DISABLED) { - // Do nothing all messages should be ignored when in this state - goto done; - } - - if (port->getPortState() == PTP_FAULTY) { - // According to spec recovery is implementation specific - eport->recoverPort(); - goto done; - } - - port->incCounter_ieee8021AsPortStatRxPdelayRequest(); - - /* Generate and send message */ - resp = new PTPMessagePathDelayResp(eport); - port->getPortIdentity(resp_id); - resp->setPortIdentity(&resp_id); - resp->setSequenceId(sequenceId); - - GPTP_LOG_DEBUG("Process PDelay Request SeqId: %u\t", sequenceId); - -#ifdef DEBUG - for (int n = 0; n < PTP_CLOCK_IDENTITY_LENGTH; ++n) { - GPTP_LOG_VERBOSE("%c", resp_id.clockIdentity[n]); - } -#endif - - this->getPortIdentity(&requestingPortIdentity_p); - resp->setRequestingPortIdentity(&requestingPortIdentity_p); - resp->setRequestReceiptTimestamp(_timestamp); - - port->getTxLock(); - resp->sendPort(eport, sourcePortIdentity); - GPTP_LOG_DEBUG("*** Sent PDelay Response message"); - port->putTxLock(); - - if( resp->getTimestamp()._version != _timestamp._version ) { - GPTP_LOG_ERROR("TX timestamp version mismatch: %u/%u", - resp->getTimestamp()._version, _timestamp._version); -#if 0 // discarding the request could lead to the peer setting the link to non-asCapable - delete resp; - goto done; -#endif - } - - resp_fwup = new PTPMessagePathDelayRespFollowUp(eport); - port->getPortIdentity(resp_fwup_id); - resp_fwup->setPortIdentity(&resp_fwup_id); - resp_fwup->setSequenceId(sequenceId); - resp_fwup->setRequestingPortIdentity(sourcePortIdentity); - resp_fwup->setResponseOriginTimestamp(resp->getTimestamp()); - long long turnaround; - turnaround = (resp->getTimestamp().seconds_ls - _timestamp.seconds_ls) - * 1000000000LL; - - GPTP_LOG_VERBOSE("Response Depart(sec): %u", - resp->getTimestamp().seconds_ls); - GPTP_LOG_VERBOSE("Request Arrival(sec): %u", _timestamp.seconds_ls); - GPTP_LOG_VERBOSE("#1 Correction Field: %Ld", turnaround); - - turnaround += resp->getTimestamp().nanoseconds; - - GPTP_LOG_VERBOSE("#2 Correction Field: %Ld", turnaround); - - turnaround -= _timestamp.nanoseconds; - - GPTP_LOG_VERBOSE("#3 Correction Field: %Ld", turnaround); - - resp_fwup->setCorrectionField(0); - resp_fwup->sendPort(eport, sourcePortIdentity); - - GPTP_LOG_DEBUG("*** Sent PDelay Response FollowUp message"); - - delete resp; - delete resp_fwup; - -done: - delete timer; - _gc = true; - return; -} - -bool PTPMessagePathDelayReq::sendPort -( EtherPort *port, PortIdentity *destIdentity ) -{ - uint32_t link_speed; - - if(port->pdelayHalted()) - return false; - - uint8_t buf_t[256]; - uint8_t *buf_ptr = buf_t + port->getPayloadOffset(); - unsigned char tspec_msg_t = 0; - memset(buf_t, 0, 256); - /* Create packet in buf */ - /* Copy in common header */ - messageLength = PTP_COMMON_HDR_LENGTH + PTP_PDELAY_REQ_LENGTH; - tspec_msg_t |= messageType & 0xF; - buildCommonHeader(buf_ptr); - port->sendEventPort - ( PTP_ETHERTYPE, buf_t, messageLength, MCAST_PDELAY, - destIdentity, &link_speed ); - port->incCounter_ieee8021AsPortStatTxPdelayRequest(); - - return getTxTimestamp( port, link_speed ); -} - -PTPMessagePathDelayResp::PTPMessagePathDelayResp -( EtherPort *port ) : PTPMessageCommon( port ) -{ - /*TODO: Why 0x7F?*/ - logMeanMessageInterval = 0x7F; - control = MESSAGE_OTHER; - messageType = PATH_DELAY_RESP_MESSAGE; - versionPTP = GPTP_VERSION; - requestingPortIdentity = new PortIdentity(); - - flags[PTP_ASSIST_BYTE] |= (0x1 << PTP_ASSIST_BIT); - - return; -} - -PTPMessagePathDelayResp::~PTPMessagePathDelayResp() -{ - delete requestingPortIdentity; -} - -void PTPMessagePathDelayResp::processMessage( CommonPort *port ) -{ - EtherPort *eport = dynamic_cast <EtherPort *> (port); - if (eport == NULL) - { - GPTP_LOG_ERROR( "Received Pdelay Resp on wrong port type" ); - _gc = true; - return; - } - - if (port->getPortState() == PTP_DISABLED) { - // Do nothing all messages should be ignored when in this state - return; - } - if (port->getPortState() == PTP_FAULTY) { - // According to spec recovery is implementation specific - eport->recoverPort(); - return; - } - - port->incCounter_ieee8021AsPortStatRxPdelayResponse(); - - if (eport->tryPDelayRxLock() != true) { - GPTP_LOG_ERROR("Failed to get PDelay RX Lock"); - return; - } - - PortIdentity resp_id; - PortIdentity oldresp_id; - uint16_t resp_port_number; - uint16_t oldresp_port_number; - - PTPMessagePathDelayResp *old_pdelay_resp = eport->getLastPDelayResp(); - if( old_pdelay_resp == NULL ) { - goto bypass_verify_duplicate; - } - - old_pdelay_resp->getPortIdentity(&oldresp_id); - oldresp_id.getPortNumber(&oldresp_port_number); - getPortIdentity(&resp_id); - resp_id.getPortNumber(&resp_port_number); - - /* In the case where we have multiple PDelay responses for the same - * PDelay request, and they come from different sources, it is necessary - * to verify if this happens 3 times (sequentially). If it does, PDelayRequests - * are halted for 5 minutes - */ - if( getSequenceId() == old_pdelay_resp->getSequenceId() ) - { - /*If the duplicates are in sequence and from different sources*/ - if( (resp_port_number != oldresp_port_number ) && ( - (eport->getLastInvalidSeqID() + 1 ) == getSequenceId() || - eport->getDuplicateRespCounter() == 0 ) ){ - GPTP_LOG_ERROR("Two responses for same Request. seqID %d. First Response Port# %hu. Second Port# %hu. Counter %d", - getSequenceId(), oldresp_port_number, resp_port_number, eport->getDuplicateRespCounter()); - - if( eport->incrementDuplicateRespCounter() ) { - GPTP_LOG_ERROR("Remote misbehaving. Stopping PDelay Requests for 5 minutes."); - eport->stopPDelay(); - eport->getClock()->addEventTimerLocked - (port, PDELAY_RESP_PEER_MISBEHAVING_TIMEOUT_EXPIRES, (int64_t)(300 * 1000000000.0)); - } - } - else { - eport->setDuplicateRespCounter(0); - } - eport->setLastInvalidSeqID(getSequenceId()); - } - else - { - eport->setDuplicateRespCounter(0); - } - -bypass_verify_duplicate: - eport->setLastPDelayResp(this); - - if (old_pdelay_resp != NULL) { - delete old_pdelay_resp; - } - - eport->putPDelayRxLock(); - _gc = false; - - return; -} - -bool PTPMessagePathDelayResp::sendPort -( EtherPort *port, PortIdentity *destIdentity ) -{ - uint8_t buf_t[256]; - uint8_t *buf_ptr = buf_t + port->getPayloadOffset(); - unsigned char tspec_msg_t = 0; - Timestamp requestReceiptTimestamp_BE; - uint32_t link_speed; - - memset(buf_t, 0, 256); - // Create packet in buf - // Copy in common header - messageLength = PTP_COMMON_HDR_LENGTH + PTP_PDELAY_RESP_LENGTH; - tspec_msg_t |= messageType & 0xF; - buildCommonHeader(buf_ptr); - requestReceiptTimestamp_BE.seconds_ms = - PLAT_htons(requestReceiptTimestamp.seconds_ms); - requestReceiptTimestamp_BE.seconds_ls = - PLAT_htonl(requestReceiptTimestamp.seconds_ls); - requestReceiptTimestamp_BE.nanoseconds = - PLAT_htonl(requestReceiptTimestamp.nanoseconds); - - // Copy in v2 PDelay_Req specific fields - requestingPortIdentity->getClockIdentityString - (buf_ptr + PTP_PDELAY_RESP_REQ_CLOCK_ID - (PTP_PDELAY_RESP_OFFSET)); - requestingPortIdentity->getPortNumberNO - ((uint16_t *) - (buf_ptr + PTP_PDELAY_RESP_REQ_PORT_ID - (PTP_PDELAY_RESP_OFFSET))); - memcpy(buf_ptr + PTP_PDELAY_RESP_SEC_MS(PTP_PDELAY_RESP_OFFSET), - &(requestReceiptTimestamp_BE.seconds_ms), - sizeof(requestReceiptTimestamp.seconds_ms)); - memcpy(buf_ptr + PTP_PDELAY_RESP_SEC_LS(PTP_PDELAY_RESP_OFFSET), - &(requestReceiptTimestamp_BE.seconds_ls), - sizeof(requestReceiptTimestamp.seconds_ls)); - memcpy(buf_ptr + PTP_PDELAY_RESP_NSEC(PTP_PDELAY_RESP_OFFSET), - &(requestReceiptTimestamp_BE.nanoseconds), - sizeof(requestReceiptTimestamp.nanoseconds)); - - GPTP_LOG_VERBOSE("PDelay Resp Timestamp: %u,%u", - requestReceiptTimestamp.seconds_ls, - requestReceiptTimestamp.nanoseconds); - - port->sendEventPort - ( PTP_ETHERTYPE, buf_t, messageLength, MCAST_PDELAY, - destIdentity, &link_speed ); - port->incCounter_ieee8021AsPortStatTxPdelayResponse(); - - return getTxTimestamp( port, link_speed ); -} - -void PTPMessagePathDelayResp::setRequestingPortIdentity -(PortIdentity * identity) -{ - *requestingPortIdentity = *identity; -} - -void PTPMessagePathDelayResp::getRequestingPortIdentity -(PortIdentity * identity) -{ - *identity = *requestingPortIdentity; -} - -PTPMessagePathDelayRespFollowUp::PTPMessagePathDelayRespFollowUp -( EtherPort *port ) : PTPMessageCommon( port ) -{ - logMeanMessageInterval = 0x7F; - control = MESSAGE_OTHER; - messageType = PATH_DELAY_FOLLOWUP_MESSAGE; - versionPTP = GPTP_VERSION; - requestingPortIdentity = new PortIdentity(); - - return; -} - -PTPMessagePathDelayRespFollowUp::~PTPMessagePathDelayRespFollowUp() -{ - delete requestingPortIdentity; -} - -#define US_PER_SEC 1000000 -void PTPMessagePathDelayRespFollowUp::processMessage -( CommonPort *port ) -{ - PTPMessagePathDelayReq *req; - PTPMessagePathDelayResp *resp; - - Timestamp remote_resp_tx_timestamp(0, 0, 0); - Timestamp request_tx_timestamp(0, 0, 0); - Timestamp remote_req_rx_timestamp(0, 0, 0); - Timestamp response_rx_timestamp(0, 0, 0); - - EtherPort *eport = dynamic_cast <EtherPort *> (port); - if (eport == NULL) - { - GPTP_LOG_ERROR( "Received Pdelay Response FollowUp on wrong " - "port type" ); - goto abort; - } - - if (port->getPortState() == PTP_DISABLED) { - // Do nothing all messages should be ignored when in this state - return; - } - if (port->getPortState() == PTP_FAULTY) { - // According to spec recovery is implementation specific - eport->recoverPort(); - return; - } - - port->incCounter_ieee8021AsPortStatRxPdelayResponseFollowUp(); - - if (eport->tryPDelayRxLock() != true) - return; - - req = eport->getLastPDelayReq(); - resp = eport->getLastPDelayResp(); - - if (req == NULL) { - /* Shouldn't happen */ - GPTP_LOG_ERROR - (">>> Received PDelay followup but no REQUEST exists"); - goto abort; - } - - if (resp == NULL) { - /* Probably shouldn't happen either */ - GPTP_LOG_ERROR - (">>> Received PDelay followup but no RESPONSE exists"); - - goto abort; - } - - if( req->getSequenceId() != sequenceId ) { - GPTP_LOG_ERROR - ( "Received PDelay FUP has different seqID than the " - "PDelay request (%d/%d)", - sequenceId, req->getSequenceId() ); - goto abort; - } - - { - PortIdentity req_id; - PortIdentity resp_id; - uint16_t resp_port_number; - uint16_t req_port_number; - - ClockIdentity resp_clkId = resp_id.getClockIdentity(); - ClockIdentity req_clkId = req_id.getClockIdentity(); - PortIdentity fup_sourcePortIdentity; - PortIdentity resp_sourcePortIdentity; - - req->getPortIdentity(&req_id); - resp->getRequestingPortIdentity(&resp_id); - - resp_id.getPortNumber(&resp_port_number); - requestingPortIdentity->getPortNumber(&req_port_number); - - resp->getPortIdentity(&resp_sourcePortIdentity); - getPortIdentity(&fup_sourcePortIdentity); - - /* - * IEEE 802.1AS, Figure 11-8, subclause 11.2.15.3 - */ - if (resp->getSequenceId() != sequenceId) { - GPTP_LOG_ERROR - ("Received PDelay Response Follow Up but cannot find " - "corresponding response"); - GPTP_LOG_ERROR( "%hu, %hu, %hu, %hu", - resp->getSequenceId(), sequenceId, - resp_port_number, req_port_number ); - - goto abort; - } - - /* - * IEEE 802.1AS, Figure 11-8, subclause 11.2.15.3 - */ - if (req_clkId != resp_clkId) { - GPTP_LOG_ERROR - ( "ClockID Resp/Req differs. PDelay Response ClockID: " - "%s PDelay Request ClockID: %s", - req_clkId.getIdentityString().c_str(), - resp_clkId.getIdentityString().c_str( )); - goto abort; - } - - /* - * IEEE 802.1AS, Figure 11-8, subclause 11.2.15.3 - */ - if (resp_port_number != req_port_number) { - GPTP_LOG_ERROR - ( "Request port number (%hu) is different from " - "Response port number (%hu)", - req_port_number, resp_port_number ); - - goto abort; - } - - /* - * IEEE 802.1AS, Figure 11-8, subclause 11.2.15.3 - */ - if (fup_sourcePortIdentity != resp_sourcePortIdentity) { - GPTP_LOG_ERROR( "Source port identity from " - "PDelay Response/FUP differ" ); - - goto abort; - } - } - - port->getClock()->deleteEventTimerLocked - (port, PDELAY_RESP_RECEIPT_TIMEOUT_EXPIRES); - - GPTP_LOG_VERBOSE("Request Sequence Id: %u", req->getSequenceId()); - GPTP_LOG_VERBOSE("Response Sequence Id: %u", resp->getSequenceId()); - GPTP_LOG_VERBOSE("Follow-Up Sequence Id: %u", sequenceId); - - int64_t link_delay; - unsigned long long turn_around; - - /* Assume that we are a two step clock, otherwise originTimestamp - may be used */ - request_tx_timestamp = req->getTimestamp(); - if( request_tx_timestamp.nanoseconds == INVALID_TIMESTAMP.nanoseconds ) - { - /* Stop processing the packet */ - goto abort; - } - - if (request_tx_timestamp.nanoseconds == - PDELAY_PENDING_TIMESTAMP.nanoseconds) { - // Defer processing - if( - eport->getLastPDelayRespFollowUp() != NULL && - eport->getLastPDelayRespFollowUp() != this ) - { - delete eport->getLastPDelayRespFollowUp(); - } - eport->setLastPDelayRespFollowUp(this); - port->getClock()->addEventTimerLocked - (port, PDELAY_DEFERRED_PROCESSING, 1000000); - goto defer; - } - remote_req_rx_timestamp = resp->getRequestReceiptTimestamp(); - response_rx_timestamp = resp->getTimestamp(); - remote_resp_tx_timestamp = responseOriginTimestamp; - - if( request_tx_timestamp._version != response_rx_timestamp._version ) { - GPTP_LOG_ERROR("RX timestamp version mismatch %d/%d", - request_tx_timestamp._version, response_rx_timestamp._version ); - goto abort; - } - - port->incPdelayCount(); - - - link_delay = - ((response_rx_timestamp.seconds_ms * 1LL - - request_tx_timestamp.seconds_ms) << 32) * 1000000000; - link_delay += - (response_rx_timestamp.seconds_ls * 1LL - - request_tx_timestamp.seconds_ls) * 1000000000; - link_delay += - (response_rx_timestamp.nanoseconds * 1LL - - request_tx_timestamp.nanoseconds); - - turn_around = - ((remote_resp_tx_timestamp.seconds_ms * 1LL - - remote_req_rx_timestamp.seconds_ms) << 32) * 1000000000; - turn_around += - (remote_resp_tx_timestamp.seconds_ls * 1LL - - remote_req_rx_timestamp.seconds_ls) * 1000000000; - turn_around += - (remote_resp_tx_timestamp.nanoseconds * 1LL - - remote_req_rx_timestamp.nanoseconds); - - // Adjust turn-around time for peer to local clock rate difference - // TODO: Are these .998 and 1.002 specifically defined in the standard? - // Should we create a define for them ? - if - ( port->getPeerRateOffset() > .998 && - port->getPeerRateOffset() < 1.002 ) { - turn_around = (int64_t) - (turn_around * port->getPeerRateOffset()); - } - - GPTP_LOG_VERBOSE - ("Turn Around Adjustment %Lf", - ((long long)turn_around * port->getPeerRateOffset()) / - 1000000000000LL); - GPTP_LOG_VERBOSE - ("Step #1: Turn Around Adjustment %Lf", - ((long long)turn_around * port->getPeerRateOffset())); - GPTP_LOG_VERBOSE("Adjusted Peer turn around is %Lu", turn_around); - - /* Subtract turn-around time from link delay after rate adjustment */ - link_delay -= turn_around; - link_delay /= 2; - GPTP_LOG_DEBUG( "Link delay: %ld ns", link_delay ); - - { - uint64_t mine_elapsed; - uint64_t theirs_elapsed; - Timestamp prev_peer_ts_mine; - Timestamp prev_peer_ts_theirs; - FrequencyRatio rate_offset; - if( port->getPeerOffset( prev_peer_ts_mine, prev_peer_ts_theirs )) { - FrequencyRatio upper_ratio_limit, lower_ratio_limit; - upper_ratio_limit = - PPM_OFFSET_TO_RATIO(UPPER_LIMIT_PPM); - lower_ratio_limit = - PPM_OFFSET_TO_RATIO(LOWER_LIMIT_PPM); - - mine_elapsed = TIMESTAMP_TO_NS(request_tx_timestamp) - - TIMESTAMP_TO_NS(prev_peer_ts_mine); - theirs_elapsed = - TIMESTAMP_TO_NS(remote_req_rx_timestamp) - - TIMESTAMP_TO_NS(prev_peer_ts_theirs); - theirs_elapsed -= port->getLinkDelay(); - theirs_elapsed += link_delay < 0 ? 0 : link_delay; - rate_offset = ((FrequencyRatio) mine_elapsed) - / theirs_elapsed; - - if( rate_offset < upper_ratio_limit && - rate_offset > lower_ratio_limit ) - port->setPeerRateOffset(rate_offset); - } - } - if( !port->setLinkDelay( link_delay )) - { - if( !eport->getAutomotiveProfile( )) - { - GPTP_LOG_ERROR( "Link delay %ld beyond " - "neighborPropDelayThresh; " - "not AsCapable", link_delay ); - port->setAsCapable( false ); - } - } else - { - if( !eport->getAutomotiveProfile( )) - port->setAsCapable( true ); - } - port->setPeerOffset( request_tx_timestamp, remote_req_rx_timestamp ); - - abort: - delete resp; - eport->setLastPDelayResp(NULL); - - _gc = true; - - defer: - eport->putPDelayRxLock(); - - return; -} - -bool PTPMessagePathDelayRespFollowUp::sendPort -( EtherPort *port, PortIdentity *destIdentity ) -{ - uint8_t buf_t[256]; - uint8_t *buf_ptr = buf_t + port->getPayloadOffset(); - unsigned char tspec_msg_t = 0; - Timestamp responseOriginTimestamp_BE; - memset(buf_t, 0, 256); - /* Create packet in buf - Copy in common header */ - messageLength = PTP_COMMON_HDR_LENGTH + PTP_PDELAY_RESP_LENGTH; - tspec_msg_t |= messageType & 0xF; - buildCommonHeader(buf_ptr); - responseOriginTimestamp_BE.seconds_ms = - PLAT_htons(responseOriginTimestamp.seconds_ms); - responseOriginTimestamp_BE.seconds_ls = - PLAT_htonl(responseOriginTimestamp.seconds_ls); - responseOriginTimestamp_BE.nanoseconds = - PLAT_htonl(responseOriginTimestamp.nanoseconds); - - // Copy in v2 PDelay_Req specific fields - requestingPortIdentity->getClockIdentityString - (buf_ptr + PTP_PDELAY_FOLLOWUP_REQ_CLOCK_ID - (PTP_PDELAY_FOLLOWUP_OFFSET)); - requestingPortIdentity->getPortNumberNO - ((uint16_t *) - (buf_ptr + PTP_PDELAY_FOLLOWUP_REQ_PORT_ID - (PTP_PDELAY_FOLLOWUP_OFFSET))); - memcpy - (buf_ptr + PTP_PDELAY_FOLLOWUP_SEC_MS(PTP_PDELAY_FOLLOWUP_OFFSET), - &(responseOriginTimestamp_BE.seconds_ms), - sizeof(responseOriginTimestamp.seconds_ms)); - memcpy - (buf_ptr + PTP_PDELAY_FOLLOWUP_SEC_LS(PTP_PDELAY_FOLLOWUP_OFFSET), - &(responseOriginTimestamp_BE.seconds_ls), - sizeof(responseOriginTimestamp.seconds_ls)); - memcpy - (buf_ptr + PTP_PDELAY_FOLLOWUP_NSEC(PTP_PDELAY_FOLLOWUP_OFFSET), - &(responseOriginTimestamp_BE.nanoseconds), - sizeof(responseOriginTimestamp.nanoseconds)); - - GPTP_LOG_VERBOSE("PDelay Resp Timestamp: %u,%u", - responseOriginTimestamp.seconds_ls, - responseOriginTimestamp.nanoseconds); - - port->sendGeneralPort(PTP_ETHERTYPE, buf_t, messageLength, MCAST_PDELAY, destIdentity); - port->incCounter_ieee8021AsPortStatTxPdelayResponseFollowUp(); - - return true; -} - -void PTPMessagePathDelayRespFollowUp::setRequestingPortIdentity -(PortIdentity * identity) -{ - *requestingPortIdentity = *identity; -} - - - PTPMessageSignalling::PTPMessageSignalling(void) -{ -} - -PTPMessageSignalling::PTPMessageSignalling -( EtherPort *port ) : PTPMessageCommon( port ) -{ - messageType = SIGNALLING_MESSAGE; - sequenceId = port->getNextSignalSequenceId(); - - targetPortIdentify = (int8_t)0xff; - - control = MESSAGE_OTHER; - - logMeanMessageInterval = 0x7F; // 802.1AS 2011 10.5.2.2.11 logMessageInterval (Integer8) -} - - PTPMessageSignalling::~PTPMessageSignalling(void) -{ -} - -void PTPMessageSignalling::setintervals(int8_t linkDelayInterval, int8_t timeSyncInterval, int8_t announceInterval) -{ - tlv.setLinkDelayInterval(linkDelayInterval); - tlv.setTimeSyncInterval(timeSyncInterval); - tlv.setAnnounceInterval(announceInterval); -} - -bool PTPMessageSignalling::sendPort -( EtherPort *port, PortIdentity *destIdentity ) -{ - uint8_t buf_t[256]; - uint8_t *buf_ptr = buf_t + port->getPayloadOffset(); - unsigned char tspec_msg_t = 0x0; - - memset(buf_t, 0, 256); - // Create packet in buf - // Copy in common header - messageLength = PTP_COMMON_HDR_LENGTH + PTP_SIGNALLING_LENGTH + sizeof(tlv); - tspec_msg_t |= messageType & 0xF; - buildCommonHeader(buf_ptr); - - memcpy(buf_ptr + PTP_SIGNALLING_TARGET_PORT_IDENTITY(PTP_SIGNALLING_OFFSET), - &targetPortIdentify, sizeof(targetPortIdentify)); - - tlv.toByteString(buf_ptr + PTP_COMMON_HDR_LENGTH + PTP_SIGNALLING_LENGTH); - - port->sendGeneralPort(PTP_ETHERTYPE, buf_t, messageLength, MCAST_OTHER, destIdentity); - - return true; -} - -void PTPMessageSignalling::processMessage( CommonPort *port ) -{ - long long unsigned int waitTime; - - GPTP_LOG_STATUS("Signalling Link Delay Interval: %d", tlv.getLinkDelayInterval()); - GPTP_LOG_STATUS("Signalling Sync Interval: %d", tlv.getTimeSyncInterval()); - GPTP_LOG_STATUS("Signalling Announce Interval: %d", tlv.getAnnounceInterval()); - - char linkDelayInterval = tlv.getLinkDelayInterval(); - char timeSyncInterval = tlv.getTimeSyncInterval(); - char announceInterval = tlv.getAnnounceInterval(); - - if (linkDelayInterval == PTPMessageSignalling::sigMsgInterval_Initial) { - port->resetInitPDelayInterval(); - - waitTime = ((long long) (pow((double)2, port->getPDelayInterval()) * 1000000000.0)); - waitTime = waitTime > EVENT_TIMER_GRANULARITY ? waitTime : EVENT_TIMER_GRANULARITY; - port->startPDelayIntervalTimer(waitTime); - } - else if (linkDelayInterval == PTPMessageSignalling::sigMsgInterval_NoSend) { - // TODO: No send functionality needs to be implemented. - GPTP_LOG_WARNING("Signal received to stop sending pDelay messages: Not implemented"); - } - else if (linkDelayInterval == PTPMessageSignalling::sigMsgInterval_NoChange) { - // Nothing to do - } - else { - port->setPDelayInterval(linkDelayInterval); - - waitTime = ((long long) (pow((double)2, port->getPDelayInterval()) * 1000000000.0)); - waitTime = waitTime > EVENT_TIMER_GRANULARITY ? waitTime : EVENT_TIMER_GRANULARITY; - port->startPDelayIntervalTimer(waitTime); - } - - if (timeSyncInterval == PTPMessageSignalling::sigMsgInterval_Initial) { - port->resetInitSyncInterval(); - - waitTime = ((long long) (pow((double)2, port->getSyncInterval()) * 1000000000.0)); - waitTime = waitTime > EVENT_TIMER_GRANULARITY ? waitTime : EVENT_TIMER_GRANULARITY; - port->startSyncIntervalTimer(waitTime); - } - else if (timeSyncInterval == PTPMessageSignalling::sigMsgInterval_NoSend) { - // TODO: No send functionality needs to be implemented. - GPTP_LOG_WARNING("Signal received to stop sending Sync messages: Not implemented"); - } - else if (timeSyncInterval == PTPMessageSignalling::sigMsgInterval_NoChange) { - // Nothing to do - } - else { - port->setSyncInterval(timeSyncInterval); - - waitTime = ((long long) (pow((double)2, port->getSyncInterval()) * 1000000000.0)); - waitTime = waitTime > EVENT_TIMER_GRANULARITY ? waitTime : EVENT_TIMER_GRANULARITY; - port->startSyncIntervalTimer(waitTime); - } - - if (!port->getAutomotiveProfile()) { - if (announceInterval == PTPMessageSignalling::sigMsgInterval_Initial) { - // TODO: Needs implementation - GPTP_LOG_WARNING("Signal received to set Announce message to initial interval: Not implemented"); - } - else if (announceInterval == PTPMessageSignalling::sigMsgInterval_NoSend) { - // TODO: No send functionality needs to be implemented. - GPTP_LOG_WARNING("Signal received to stop sending Announce messages: Not implemented"); - } - else if (announceInterval == PTPMessageSignalling::sigMsgInterval_NoChange) { - // Nothing to do - } - else { - port->setAnnounceInterval(announceInterval); - - waitTime = ((long long) (pow((double)2, port->getAnnounceInterval()) * 1000000000.0)); - waitTime = waitTime > EVENT_TIMER_GRANULARITY ? waitTime : EVENT_TIMER_GRANULARITY; - port->startAnnounceIntervalTimer(waitTime); - } - } -} |