From a6163888f3c56123b1db313743c6147ba498732c Mon Sep 17 00:00:00 2001 From: Yuval Adam Date: Fri, 8 Aug 2014 14:42:07 +0300 Subject: Add third_party libs --- third_party/ptpd-1.1.0/src/protocol.c | 865 ++++++++++++++++++++++++++++++++++ 1 file changed, 865 insertions(+) create mode 100644 third_party/ptpd-1.1.0/src/protocol.c (limited to 'third_party/ptpd-1.1.0/src/protocol.c') diff --git a/third_party/ptpd-1.1.0/src/protocol.c b/third_party/ptpd-1.1.0/src/protocol.c new file mode 100644 index 0000000..4db8e95 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/protocol.c @@ -0,0 +1,865 @@ +/** + * @file protocol.c + * @date Wed Jun 23 09:40:39 2010 + * + * @brief The code that handles the IEEE-1588 protocol and state machine + * + * + */ + +#include "ptpd.h" + +Boolean doInit(RunTimeOpts *, PtpClock *); +void doState(RunTimeOpts *, PtpClock *); +void toState(UInteger8, RunTimeOpts *, PtpClock *); + +void handle(RunTimeOpts *, PtpClock *); +void handleSync(MsgHeader *, Octet *, ssize_t, TimeInternal *, Boolean, RunTimeOpts *, PtpClock *); +void handleFollowUp(MsgHeader *, Octet *, ssize_t, Boolean, RunTimeOpts *, PtpClock *); +void handleDelayReq(MsgHeader *, Octet *, ssize_t, TimeInternal *, Boolean, RunTimeOpts *, PtpClock *); +void handleDelayResp(MsgHeader *, Octet *, ssize_t, Boolean, RunTimeOpts *, PtpClock *); +void handleManagement(MsgHeader *, Octet *, ssize_t, Boolean, RunTimeOpts *, PtpClock *); + +void issueSync(RunTimeOpts *, PtpClock *); +void issueFollowup(TimeInternal *, RunTimeOpts *, PtpClock *); +void issueDelayReq(RunTimeOpts *, PtpClock *); +void issueDelayResp(TimeInternal *, MsgHeader *, RunTimeOpts *, PtpClock *); +void issueManagement(MsgHeader *, MsgManagement *, RunTimeOpts *, PtpClock *); + +MsgSync *addForeign(Octet *, MsgHeader *, PtpClock *); + +/* Initialize the PTPD stack. + * + * NB: This function was added by TI! + */ +void protocol_first(RunTimeOpts *rtOpts, PtpClock *ptpClock) +{ + DBG("event POWERUP\n"); + + toState(PTP_INITIALIZING, rtOpts, ptpClock); + + if(ptpClock->port_state != PTP_INITIALIZING) + doState(rtOpts, ptpClock); + else if(!doInit(rtOpts, ptpClock)) + return; + + if(ptpClock->message_activity) + DBGV("activity\n"); +#if 0 + else + DBGV("no activity\n"); +#endif +} + +/* Perform one iteration of the PTPD state machine. This function does not + * block and may be used in non-RTOS systems where the main loop needs to do + * more than process the PTPD stack. + * + * NB: This function was added by TI! + */ +void protocol_loop(RunTimeOpts *rtOpts, PtpClock *ptpClock) +{ + if(ptpClock->port_state != PTP_INITIALIZING) + doState(rtOpts, ptpClock); + else if(!doInit(rtOpts, ptpClock)) + return; + + if(ptpClock->message_activity) + DBGV("activity\n"); +#if 0 + else + DBGV("no activity\n"); +#endif +} + +/* loop forever. doState() has a switch for the actions and events to be + checked for 'port_state'. the actions and events may or may not change + 'port_state' by calling toState(), but once they are done we loop around + again and perform the actions required for the new 'port_state'. */ +void +protocol(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + DBG("event POWERUP\n"); + + toState(PTP_INITIALIZING, rtOpts, ptpClock); + + DBGV("Debug Initializing..."); + + for (;;) { + if (ptpClock->port_state != PTP_INITIALIZING) + doState(rtOpts, ptpClock); + else if (!doInit(rtOpts, ptpClock)) + return; + + if (ptpClock->message_activity) + DBGV("activity\n"); + else + DBGV("no activity\n"); + } +} + +Boolean +doInit(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + DBG("manufacturerIdentity: %s\n", MANUFACTURER_ID); + + /* initialize networking */ + netShutdown(&ptpClock->netPath); + if (!netInit(&ptpClock->netPath, rtOpts, ptpClock)) { + ERROR("failed to initialize network\n"); + toState(PTP_FAULTY, rtOpts, ptpClock); + return FALSE; + } + /* initialize other stuff */ + initData(rtOpts, ptpClock); + initTimer(); + initClock(rtOpts, ptpClock); + m1(ptpClock); + msgPackHeader(ptpClock->msgObuf, ptpClock); + + DBG("sync message interval: %d\n", PTP_SYNC_INTERVAL_TIMEOUT(ptpClock->sync_interval)); + DBG("clock identifier: %s\n", ptpClock->clock_identifier); + DBG("256*log2(clock variance): %d\n", ptpClock->clock_variance); + DBG("clock stratum: %d\n", ptpClock->clock_stratum); + DBG("clock preferred?: %s\n", ptpClock->preferred ? "yes" : "no"); + DBG("bound interface name: %s\n", rtOpts->ifaceName); + DBG("communication technology: %d\n", ptpClock->port_communication_technology); + DBG("uuid: %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n", + ptpClock->port_uuid_field[0], ptpClock->port_uuid_field[1], ptpClock->port_uuid_field[2], + ptpClock->port_uuid_field[3], ptpClock->port_uuid_field[4], ptpClock->port_uuid_field[5]); + DBG("PTP subdomain name: %s\n", ptpClock->subdomain_name); + DBG("subdomain address: %hhx.%hhx.%hhx.%hhx\n", + ptpClock->subdomain_address[0], ptpClock->subdomain_address[1], + ptpClock->subdomain_address[2], ptpClock->subdomain_address[3]); + DBG("event port address: %hhx %hhx\n", + ptpClock->event_port_address[0], ptpClock->event_port_address[1]); + DBG("general port address: %hhx %hhx\n", + ptpClock->general_port_address[0], ptpClock->general_port_address[1]); + + toState(PTP_LISTENING, rtOpts, ptpClock); + return TRUE; +} + +/* handle actions and events for 'port_state' */ +void +doState(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + UInteger8 state; + + ptpClock->message_activity = FALSE; + + switch (ptpClock->port_state) { + case PTP_LISTENING: + case PTP_PASSIVE: + case PTP_SLAVE: + case PTP_MASTER: + if (ptpClock->record_update) { + ptpClock->record_update = FALSE; + state = bmc(ptpClock->foreign, rtOpts, ptpClock); + if (state != ptpClock->port_state) + toState(state, rtOpts, ptpClock); + } + break; + + default: + break; + } + + switch (ptpClock->port_state) { + case PTP_FAULTY: + /* imaginary troubleshooting */ + + DBG("event FAULT_CLEARED\n"); + toState(PTP_INITIALIZING, rtOpts, ptpClock); + return; + + case PTP_LISTENING: + case PTP_PASSIVE: + case PTP_UNCALIBRATED: + case PTP_SLAVE: + handle(rtOpts, ptpClock); + + if (timerExpired(SYNC_RECEIPT_TIMER, ptpClock->itimer)) { + DBG("event SYNC_RECEIPT_TIMEOUT_EXPIRES\n"); + ptpClock->number_foreign_records = 0; + ptpClock->foreign_record_i = 0; + if (!rtOpts->slaveOnly && ptpClock->clock_stratum != 255) { + m1(ptpClock); + toState(PTP_MASTER, rtOpts, ptpClock); + } else if (ptpClock->port_state != PTP_LISTENING) + toState(PTP_LISTENING, rtOpts, ptpClock); + } + break; + + case PTP_MASTER: + if (timerExpired(SYNC_INTERVAL_TIMER, ptpClock->itimer)) { + DBGV("event SYNC_INTERVAL_TIMEOUT_EXPIRES\n"); + issueSync(rtOpts, ptpClock); + } + handle(rtOpts, ptpClock); + + if (rtOpts->slaveOnly || ptpClock->clock_stratum == 255) + toState(PTP_LISTENING, rtOpts, ptpClock); + + break; + + case PTP_DISABLED: + handle(rtOpts, ptpClock); + break; + + default: + DBG("do unrecognized state\n"); + break; + } +} + +/* perform actions required when leaving 'port_state' and entering 'state' */ +void +toState(UInteger8 state, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + ptpClock->message_activity = TRUE; + + /* leaving state tasks */ + switch (ptpClock->port_state) { + case PTP_MASTER: + timerStop(SYNC_INTERVAL_TIMER, ptpClock->itimer); + timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer); + break; + + case PTP_SLAVE: + initClock(rtOpts, ptpClock); + break; + + default: + break; + } + + /* entering state tasks */ + switch (state) { + case PTP_INITIALIZING: + DBG("state PTP_INITIALIZING\n"); + timerStop(SYNC_RECEIPT_TIMER, ptpClock->itimer); + + ptpClock->port_state = PTP_INITIALIZING; + break; + + case PTP_FAULTY: + DBG("state PTP_FAULTY\n"); + timerStop(SYNC_RECEIPT_TIMER, ptpClock->itimer); + + ptpClock->port_state = PTP_FAULTY; + break; + + case PTP_DISABLED: + DBG("state change to PTP_DISABLED\n"); + timerStop(SYNC_RECEIPT_TIMER, ptpClock->itimer); + + ptpClock->port_state = PTP_DISABLED; + break; + + case PTP_LISTENING: + DBG("state PTP_LISTENING\n"); + + timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer); + + ptpClock->port_state = PTP_LISTENING; + break; + + case PTP_MASTER: + DBG("state PTP_MASTER\n"); + + if (ptpClock->port_state != PTP_PRE_MASTER) + timerStart(SYNC_INTERVAL_TIMER, PTP_SYNC_INTERVAL_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer); + + timerStop(SYNC_RECEIPT_TIMER, ptpClock->itimer); + + ptpClock->port_state = PTP_MASTER; + break; + + case PTP_PASSIVE: + DBG("state PTP_PASSIVE\n"); + ptpClock->port_state = PTP_PASSIVE; + break; + + case PTP_UNCALIBRATED: + DBG("state PTP_UNCALIBRATED\n"); + ptpClock->port_state = PTP_UNCALIBRATED; + break; + + case PTP_SLAVE: + DBG("state PTP_PTP_SLAVE\n"); + + initClock(rtOpts, ptpClock); + + /* + * R is chosen to allow a few syncs before we first get a + * one-way delay estimate + */ + /* + * this is to allow the offset filter to fill for an + * accurate initial clock reset + */ + ptpClock->Q = 0; + ptpClock->R = getRand(&ptpClock->random_seed) % 4 + 4; + DBG("Q = %d, R = %d\n", ptpClock->Q, ptpClock->R); + + ptpClock->waitingForFollow = FALSE; + ptpClock->delay_req_send_time.seconds = 0; + ptpClock->delay_req_send_time.nanoseconds = 0; + ptpClock->delay_req_receive_time.seconds = 0; + ptpClock->delay_req_receive_time.nanoseconds = 0; + + timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer); + + ptpClock->port_state = PTP_SLAVE; + break; + + default: + DBG("to unrecognized state\n"); + break; + } + + NOTIFY("Port state changed to %s\n", translatePortState(ptpClock)); + + if (rtOpts->displayStats) + displayStats(rtOpts, ptpClock); +} + +/* check and handle received messages */ +void +handle(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + int ret; + ssize_t length; + Boolean isFromSelf; + TimeInternal time = {0, 0}; + + if (!ptpClock->message_activity) { + ret = netSelect(0, &ptpClock->netPath); + if (ret < 0) { + PERROR("failed to poll sockets"); + toState(PTP_FAULTY, rtOpts, ptpClock); + return; + } else if (!ret) { + DBGV("handle: nothing\n"); + return; + } + /* else length > 0 */ + } + DBGV("handle: something\n"); + + length = netRecvEvent(ptpClock->msgIbuf, &time, &ptpClock->netPath); + if (length < 0) { + PERROR("failed to receive on the event socket"); + toState(PTP_FAULTY, rtOpts, ptpClock); + return; + } else if (!length) { + length = netRecvGeneral(ptpClock->msgIbuf, &ptpClock->netPath); + if (length < 0) { + PERROR("failed to receive on the general socket"); + toState(PTP_FAULTY, rtOpts, ptpClock); + return; + } else if (!length) + return; + } + ptpClock->message_activity = TRUE; + + if (!msgPeek(ptpClock->msgIbuf, length)) + return; + + if (length < HEADER_LENGTH) { + ERROR("message shorter than header length\n"); + toState(PTP_FAULTY, rtOpts, ptpClock); + return; + } + msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader); + + DBGV("event Receipt of Message\n" + " version %d\n" + " type %d\n" + " uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n" + " sequence %d\n" + " time %us %dns\n", + ptpClock->msgTmpHeader.versionPTP, + ptpClock->msgTmpHeader.control, + ptpClock->msgTmpHeader.sourceUuid[0], ptpClock->msgTmpHeader.sourceUuid[1], + ptpClock->msgTmpHeader.sourceUuid[2], ptpClock->msgTmpHeader.sourceUuid[3], + ptpClock->msgTmpHeader.sourceUuid[4], ptpClock->msgTmpHeader.sourceUuid[5], + ptpClock->msgTmpHeader.sequenceId, + time.seconds, time.nanoseconds); + + if (ptpClock->msgTmpHeader.versionPTP != VERSION_PTP) { + DBGV("ignore version %d message\n", ptpClock->msgTmpHeader.versionPTP); + return; + } + if (memcmp(ptpClock->msgTmpHeader.subdomain, ptpClock->subdomain_name, + PTP_SUBDOMAIN_NAME_LENGTH)) { + DBGV("ignore message from subdomain %s\n", ptpClock->msgTmpHeader.subdomain); + return; + } + isFromSelf = ptpClock->msgTmpHeader.sourceCommunicationTechnology == ptpClock->port_communication_technology + && ptpClock->msgTmpHeader.sourcePortId == ptpClock->port_id_field + && !memcmp(ptpClock->msgTmpHeader.sourceUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH); + + /* + * subtract the inbound latency adjustment if it is not a loop back + * and the time stamp seems reasonable + */ + if (!isFromSelf && time.seconds > 0) + subTime(&time, &time, &rtOpts->inboundLatency); + + switch (ptpClock->msgTmpHeader.control) { + case PTP_SYNC_MESSAGE: + handleSync(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, isFromSelf, rtOpts, ptpClock); + break; + + case PTP_FOLLOWUP_MESSAGE: + handleFollowUp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock); + break; + + case PTP_DELAY_REQ_MESSAGE: + handleDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, isFromSelf, rtOpts, ptpClock); + break; + + case PTP_DELAY_RESP_MESSAGE: + handleDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock); + break; + + case PTP_MANAGEMENT_MESSAGE: + handleManagement(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock); + break; + + default: + DBG("handle: unrecognized message\n"); + break; + } +} + +void +handleSync(MsgHeader * header, Octet * msgIbuf, ssize_t length, TimeInternal * time, Boolean isFromSelf, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + MsgSync *sync; + TimeInternal originTimestamp; + + if (length < SYNC_PACKET_LENGTH) { + ERROR("short sync message\n"); + toState(PTP_FAULTY, rtOpts, ptpClock); + return; + } + switch (ptpClock->port_state) { + case PTP_FAULTY: + case PTP_INITIALIZING: + case PTP_DISABLED: + DBGV("handleSync: disreguard\n"); + return; + + case PTP_UNCALIBRATED: + case PTP_SLAVE: + if (isFromSelf) { + DBG("handleSync: ignore from self\n"); + return; + } + if (getFlag(header->flags, PTP_SYNC_BURST) && !ptpClock->burst_enabled) + return; + + DBGV("handleSync: looking for uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n", + ptpClock->parent_uuid[0], ptpClock->parent_uuid[1], ptpClock->parent_uuid[2], + ptpClock->parent_uuid[3], ptpClock->parent_uuid[4], ptpClock->parent_uuid[5]); + + if (header->sequenceId > ptpClock->parent_last_sync_sequence_number + && header->sourceCommunicationTechnology == ptpClock->parent_communication_technology + && header->sourcePortId == ptpClock->parent_port_id + && !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH)) { + /* addForeign() takes care of msgUnpackSync() */ + ptpClock->record_update = TRUE; + sync = addForeign(ptpClock->msgIbuf, &ptpClock->msgTmpHeader, ptpClock); + + if (sync->syncInterval != ptpClock->sync_interval) { + DBGV("message's sync interval is %d, but clock's is %d\n", sync->syncInterval, ptpClock->sync_interval); + /* + * spec recommends handling a sync interval + * discrepancy as a fault + */ + } + ptpClock->sync_receive_time.seconds = time->seconds; + ptpClock->sync_receive_time.nanoseconds = time->nanoseconds; + + if (!getFlag(header->flags, PTP_ASSIST)) { + ptpClock->waitingForFollow = FALSE; + + toInternalTime(&originTimestamp, &sync->originTimestamp, &ptpClock->halfEpoch); + updateOffset(&originTimestamp, &ptpClock->sync_receive_time, + &ptpClock->ofm_filt, rtOpts, ptpClock); + updateClock(rtOpts, ptpClock); + } else { + ptpClock->waitingForFollow = TRUE; + } + + s1(header, sync, ptpClock); + + if (!(--ptpClock->R)) { + issueDelayReq(rtOpts, ptpClock); + + ptpClock->Q = 0; + ptpClock->R = getRand(&ptpClock->random_seed) % (PTP_DELAY_REQ_INTERVAL - 2) + 2; + DBG("Q = %d, R = %d\n", ptpClock->Q, ptpClock->R); + } + DBGV("SYNC_RECEIPT_TIMER reset\n"); + timerStart(SYNC_RECEIPT_TIMER, PTP_SYNC_RECEIPT_TIMEOUT(ptpClock->sync_interval), ptpClock->itimer); + +#ifndef NO_FILE_SYSTEM + if (rtOpts->recordFP != NULL) + fprintf(rtOpts->recordFP, "%d %llu\n", + header->sequenceId, + ((time->seconds * 1000000000ULL) + + time->nanoseconds)); +#endif + } else { + DBGV("handleSync: unwanted\n"); + } + + case PTP_MASTER: + default: + if (header->sourceCommunicationTechnology == ptpClock->clock_communication_technology + || header->sourceCommunicationTechnology == PTP_DEFAULT + || ptpClock->clock_communication_technology == PTP_DEFAULT) { + if (!isFromSelf) { + ptpClock->record_update = TRUE; + addForeign(ptpClock->msgIbuf, &ptpClock->msgTmpHeader, ptpClock); + } else if (ptpClock->port_state == PTP_MASTER && ptpClock->clock_followup_capable) { + addTime(time, time, &rtOpts->outboundLatency); + issueFollowup(time, rtOpts, ptpClock); + } + } + break; + } +} + +void +handleFollowUp(MsgHeader * header, Octet * msgIbuf, ssize_t length, Boolean isFromSelf, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + MsgFollowUp *follow; + TimeInternal preciseOriginTimestamp; + + if (length < FOLLOW_UP_PACKET_LENGTH) { + ERROR("short folow up message\n"); + toState(PTP_FAULTY, rtOpts, ptpClock); + return; + } + switch (ptpClock->port_state) { + case PTP_SLAVE: + if (isFromSelf) { + DBG("handleFollowUp: ignore from self\n"); + return; + } + if (getFlag(header->flags, PTP_SYNC_BURST) && !ptpClock->burst_enabled) + return; + + DBGV("handleFollowUp: looking for uuid %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n", + ptpClock->parent_uuid[0], ptpClock->parent_uuid[1], ptpClock->parent_uuid[2], + ptpClock->parent_uuid[3], ptpClock->parent_uuid[4], ptpClock->parent_uuid[5]); + + follow = &ptpClock->msgTmp.follow; + msgUnpackFollowUp(ptpClock->msgIbuf, follow); + + if (ptpClock->waitingForFollow + && follow->associatedSequenceId == ptpClock->parent_last_sync_sequence_number + && header->sourceCommunicationTechnology == ptpClock->parent_communication_technology + && header->sourcePortId == ptpClock->parent_port_id + && !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH)) { + ptpClock->waitingForFollow = FALSE; + + toInternalTime(&preciseOriginTimestamp, &follow->preciseOriginTimestamp, &ptpClock->halfEpoch); + updateOffset(&preciseOriginTimestamp, &ptpClock->sync_receive_time, + &ptpClock->ofm_filt, rtOpts, ptpClock); + updateClock(rtOpts, ptpClock); + } else { + DBGV("handleFollowUp: unwanted\n"); + } + break; + + default: + DBGV("handleFollowUp: disreguard\n"); + return; + } +} + +void +handleDelayReq(MsgHeader * header, Octet * msgIbuf, ssize_t length, TimeInternal * time, Boolean isFromSelf, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + if (length < DELAY_REQ_PACKET_LENGTH) { + ERROR("short delay request message\n"); + toState(PTP_FAULTY, rtOpts, ptpClock); + return; + } + switch (ptpClock->port_state) { + case PTP_MASTER: + if (isFromSelf) { + DBG("handleDelayReq: ignore from self\n"); + return; + } + if (header->sourceCommunicationTechnology == ptpClock->clock_communication_technology + || header->sourceCommunicationTechnology == PTP_DEFAULT + || ptpClock->clock_communication_technology == PTP_DEFAULT) { + issueDelayResp(time, &ptpClock->msgTmpHeader, rtOpts, ptpClock); + } + break; + + case PTP_SLAVE: + if (isFromSelf) { + DBG("handleDelayReq: self\n"); + + ptpClock->delay_req_send_time.seconds = time->seconds; + ptpClock->delay_req_send_time.nanoseconds = time->nanoseconds; + + addTime(&ptpClock->delay_req_send_time, &ptpClock->delay_req_send_time, &rtOpts->outboundLatency); + + if (ptpClock->delay_req_receive_time.seconds) { + updateDelay(&ptpClock->delay_req_send_time, + &ptpClock->delay_req_receive_time, + &ptpClock->owd_filt, rtOpts, + ptpClock); + + ptpClock->delay_req_send_time.seconds = 0; + ptpClock->delay_req_send_time.nanoseconds = 0; + ptpClock->delay_req_receive_time.seconds = 0; + ptpClock->delay_req_receive_time.nanoseconds = 0; + } + } + break; + + default: + DBGV("handleDelayReq: disreguard\n"); + return; + } +} + +void +handleDelayResp(MsgHeader * header, Octet * msgIbuf, ssize_t length, Boolean isFromSelf, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + MsgDelayResp *resp; + + if (length < DELAY_RESP_PACKET_LENGTH) { + ERROR("short delay request message\n"); + toState(PTP_FAULTY, rtOpts, ptpClock); + return; + } + switch (ptpClock->port_state) { + case PTP_SLAVE: + if (isFromSelf) { + DBG("handleDelayResp: ignore from self\n"); + return; + } + resp = &ptpClock->msgTmp.resp; + msgUnpackDelayResp(ptpClock->msgIbuf, resp); + + if (ptpClock->sentDelayReq + && resp->requestingSourceSequenceId == ptpClock->sentDelayReqSequenceId + && resp->requestingSourceCommunicationTechnology == ptpClock->port_communication_technology + && resp->requestingSourcePortId == ptpClock->port_id_field + && !memcmp(resp->requestingSourceUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH) + && header->sourceCommunicationTechnology == ptpClock->parent_communication_technology + && header->sourcePortId == ptpClock->parent_port_id + && !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH)) { + ptpClock->sentDelayReq = FALSE; + + toInternalTime(&ptpClock->delay_req_receive_time, &resp->delayReceiptTimestamp, &ptpClock->halfEpoch); + + if (ptpClock->delay_req_send_time.seconds) { + updateDelay(&ptpClock->delay_req_send_time, &ptpClock->delay_req_receive_time, + &ptpClock->owd_filt, rtOpts, ptpClock); + + ptpClock->delay_req_send_time.seconds = 0; + ptpClock->delay_req_send_time.nanoseconds = 0; + ptpClock->delay_req_receive_time.seconds = 0; + ptpClock->delay_req_receive_time.nanoseconds = 0; + } + } else { + DBGV("handleDelayResp: unwanted\n"); + } + break; + + default: + DBGV("handleDelayResp: disreguard\n"); + return; + } +} + +void +handleManagement(MsgHeader * header, Octet * msgIbuf, ssize_t length, Boolean isFromSelf, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + MsgManagement *manage; + + UInteger8 state; + + if (ptpClock->port_state == PTP_INITIALIZING) + return; + + manage = &ptpClock->msgTmp.manage; + msgUnpackManagement(ptpClock->msgIbuf, manage); + + if ((manage->targetCommunicationTechnology == ptpClock->clock_communication_technology + && !memcmp(manage->targetUuid, ptpClock->clock_uuid_field, PTP_UUID_LENGTH)) + || ((manage->targetCommunicationTechnology == PTP_DEFAULT + || manage->targetCommunicationTechnology == ptpClock->clock_communication_technology) + && !sum(manage->targetUuid, PTP_UUID_LENGTH))) { + switch (manage->managementMessageKey) { + case PTP_MM_OBTAIN_IDENTITY: + case PTP_MM_GET_DEFAULT_DATA_SET: + case PTP_MM_GET_CURRENT_DATA_SET: + case PTP_MM_GET_PARENT_DATA_SET: + case PTP_MM_GET_PORT_DATA_SET: + case PTP_MM_GET_GLOBAL_TIME_DATA_SET: + case PTP_MM_GET_FOREIGN_DATA_SET: + issueManagement(header, manage, rtOpts, ptpClock); + break; + + default: + ptpClock->record_update = TRUE; + state = msgUnloadManagement(ptpClock->msgIbuf, manage, ptpClock, rtOpts); + if (state != ptpClock->port_state) + toState(state, rtOpts, ptpClock); + break; + } + } else { + DBG("handleManagement: unwanted\n"); + } +} + +/* pack and send various messages */ +void +issueSync(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + TimeInternal internalTime; + TimeRepresentation originTimestamp; + + ++ptpClock->last_sync_event_sequence_number; + ptpClock->grandmaster_sequence_number = ptpClock->last_sync_event_sequence_number; + + getTime(&internalTime); + fromInternalTime(&internalTime, &originTimestamp, ptpClock->halfEpoch); + msgPackSync(ptpClock->msgObuf, FALSE, &originTimestamp, ptpClock); + + if (!netSendEvent(ptpClock->msgObuf, SYNC_PACKET_LENGTH, &ptpClock->netPath)) + toState(PTP_FAULTY, rtOpts, ptpClock); + else + DBGV("sent sync message\n"); +} + +void +issueFollowup(TimeInternal * time, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + TimeRepresentation preciseOriginTimestamp; + + ++ptpClock->last_general_event_sequence_number; + + fromInternalTime(time, &preciseOriginTimestamp, ptpClock->halfEpoch); + msgPackFollowUp(ptpClock->msgObuf, ptpClock->last_sync_event_sequence_number, &preciseOriginTimestamp, ptpClock); + + if (!netSendGeneral(ptpClock->msgObuf, FOLLOW_UP_PACKET_LENGTH, &ptpClock->netPath)) + toState(PTP_FAULTY, rtOpts, ptpClock); + else + DBGV("sent followup message\n"); +} + +void +issueDelayReq(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + TimeInternal internalTime; + TimeRepresentation originTimestamp; + + ptpClock->sentDelayReq = TRUE; + ptpClock->sentDelayReqSequenceId = ++ptpClock->last_sync_event_sequence_number; + + getTime(&internalTime); + fromInternalTime(&internalTime, &originTimestamp, ptpClock->halfEpoch); + msgPackDelayReq(ptpClock->msgObuf, FALSE, &originTimestamp, ptpClock); + + if (!netSendEvent(ptpClock->msgObuf, DELAY_REQ_PACKET_LENGTH, &ptpClock->netPath)) + toState(PTP_FAULTY, rtOpts, ptpClock); + else + DBGV("sent delay request message\n"); +} + +void +issueDelayResp(TimeInternal * time, MsgHeader * header, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + TimeRepresentation delayReceiptTimestamp; + + ++ptpClock->last_general_event_sequence_number; + + fromInternalTime(time, &delayReceiptTimestamp, ptpClock->halfEpoch); + msgPackDelayResp(ptpClock->msgObuf, header, &delayReceiptTimestamp, ptpClock); + + if (!netSendGeneral(ptpClock->msgObuf, DELAY_RESP_PACKET_LENGTH, &ptpClock->netPath)) + toState(PTP_FAULTY, rtOpts, ptpClock); + else + DBGV("sent delay response message\n"); +} + +void +issueManagement(MsgHeader * header, MsgManagement * manage, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + UInteger16 length; + + ++ptpClock->last_general_event_sequence_number; + + if (!(length = msgPackManagementResponse(ptpClock->msgObuf, header, manage, ptpClock))) + return; + + if (!netSendGeneral(ptpClock->msgObuf, length, &ptpClock->netPath)) + toState(PTP_FAULTY, rtOpts, ptpClock); + else + DBGV("sent management message\n"); +} + +/* add or update an entry in the foreign master data set */ +MsgSync * +addForeign(Octet * buf, MsgHeader * header, PtpClock * ptpClock) +{ + int i, j; + Boolean found = FALSE; + + DBGV("updateForeign\n"); + + j = ptpClock->foreign_record_best; + for (i = 0; i < ptpClock->number_foreign_records; ++i) { + if (header->sourceCommunicationTechnology == ptpClock->foreign[j].foreign_master_communication_technology + && header->sourcePortId == ptpClock->foreign[j].foreign_master_port_id + && !memcmp(header->sourceUuid, ptpClock->foreign[j].foreign_master_uuid, PTP_UUID_LENGTH)) { + ++ptpClock->foreign[j].foreign_master_syncs; + found = TRUE; + DBGV("updateForeign: update record %d\n", j); + break; + } + j = (j + 1) % ptpClock->number_foreign_records; + } + + if (!found) { + if (ptpClock->number_foreign_records < ptpClock->max_foreign_records) + ++ptpClock->number_foreign_records; + + j = ptpClock->foreign_record_i; + + ptpClock->foreign[j].foreign_master_communication_technology = + header->sourceCommunicationTechnology; + ptpClock->foreign[j].foreign_master_port_id = + header->sourcePortId; + memcpy(ptpClock->foreign[j].foreign_master_uuid, + header->sourceUuid, PTP_UUID_LENGTH); + + DBG("updateForeign: new record (%d,%d) %d %d %02hhx:%02hhx:%02hhx:%02hhx:%02hhx:%02hhx\n", + ptpClock->foreign_record_i, ptpClock->number_foreign_records, + ptpClock->foreign[j].foreign_master_communication_technology, + ptpClock->foreign[j].foreign_master_port_id, + ptpClock->foreign[j].foreign_master_uuid[0], ptpClock->foreign[j].foreign_master_uuid[1], + ptpClock->foreign[j].foreign_master_uuid[2], ptpClock->foreign[j].foreign_master_uuid[3], + ptpClock->foreign[j].foreign_master_uuid[4], ptpClock->foreign[j].foreign_master_uuid[5]); + + ptpClock->foreign_record_i = (ptpClock->foreign_record_i + 1) % ptpClock->max_foreign_records; + } + msgUnpackHeader(buf, &ptpClock->foreign[j].header); + msgUnpackSync(buf, &ptpClock->foreign[j].sync); + + return &ptpClock->foreign[j].sync; +} -- cgit v1.3.1