void probe(PtpClock *ptpClock) { UInteger16 i; UInteger16 length; TimeInternal interval, now, finish; /* check */ if(ptpClock->runTimeOpts.probe_management_key == PTP_MM_UPDATE_DEFAULT_DATA_SET || ptpClock->runTimeOpts.probe_management_key == PTP_MM_UPDATE_GLOBAL_TIME_PROPERTIES || ptpClock->runTimeOpts.probe_management_key == PTP_MM_SET_SYNC_INTERVAL) { ERROR("send not supported for that management message\n"); return; } /* init */ if(!netInit(ptpClock)) { ERROR("failed to initialize network\n"); return; } initData(ptpClock); msgPackHeader(ptpClock->msgObuf, ptpClock); memset(&ptpClock->msgTmp.manage, 0, sizeof(MsgManagement)); ptpClock->msgTmp.manage.targetCommunicationTechnology = PTP_DEFAULT; /* send */ for(i = 0; i < KEY_ARRAY_LEN; ++i) { if(ptpClock->runTimeOpts.probe_management_key > 0) { ptpClock->msgTmp.manage.managementMessageKey = ptpClock->runTimeOpts.probe_management_key; ptpClock->msgTmp.manage.recordKey = ptpClock->runTimeOpts.probe_record_key; } else ptpClock->msgTmp.manage.managementMessageKey = management_key_array[i]; if(!(length = msgPackManagement(ptpClock->msgObuf, &ptpClock->msgTmp.manage, ptpClock))) { ERROR("failed to pack management message\n"); return; } printf("\n(sending managementMessageKey %hhu)\n", ptpClock->msgTmp.manage.managementMessageKey); if(!netSendGeneral(ptpClock->msgObuf, length, ptpClock)) { ERROR("failed to send message\n"); return; } if(ptpClock->runTimeOpts.probe_management_key > 0) break; } timerNow(&finish); finish.seconds += PTP_SYNC_INTERVAL_TIMEOUT(ptpClock->sync_interval); for(;;) { interval.seconds = PTP_SYNC_INTERVAL_TIMEOUT(ptpClock->sync_interval); interval.nanoseconds = 0; netSelect(&interval, ptpClock); netRecvEvent(ptpClock->msgIbuf, NULL, ptpClock); if(netRecvGeneral(ptpClock->msgIbuf, ptpClock)) { msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader); if(ptpClock->msgTmpHeader.control == PTP_MANAGEMENT_MESSAGE) { msgUnpackManagement(ptpClock->msgIbuf, &ptpClock->msgTmp.manage); msgUnpackManagementPayload(ptpClock->msgIbuf, &ptpClock->msgTmp.manage); displayManagement(&ptpClock->msgTmpHeader, &ptpClock->msgTmp.manage); } fflush(stdout); } timerNow(&now); if( now.seconds > finish.seconds || (now.seconds == finish.seconds && now.nanoseconds > finish.nanoseconds) ) break; } /* done */ printf("\n"); ptpdShutdown(); exit(0); }
/* check and handle received messages */ void handle(PtpClock *ptpClock) { int ret; ssize_t length; Boolean isFromSelf; Boolean isEvent; Boolean badTime = FALSE; TimeInternal time = { 0, 0 }; if(!ptpClock->message_activity) { ret = netSelect(0, ptpClock); if(ret < 0) { PERROR("failed to poll sockets"); toState(PTP_FAULTY, ptpClock); return; } else if(!ret) { DBGV("handle: nothing\n"); return; } /* else length > 0 */ } DBGV("handle: something\n"); isEvent = TRUE; length = netRecvEvent(ptpClock->msgIbuf, ptpClock->delayedTiming ? NULL : &time, ptpClock); if(length < 0) { PERROR("failed to receive on the event socket"); toState(PTP_FAULTY, ptpClock); return; } else if(!length) { isEvent = FALSE; length = netRecvGeneral(ptpClock->msgIbuf, ptpClock); if(length < 0) { PERROR("failed to receive on the general socket"); toState(PTP_FAULTY, 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, ptpClock); return; } msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader); if(isEvent && ptpClock->delayedTiming) { /* query hardware for matching receive time stamp */ if(!getReceiveTime(&time, ptpClock->msgTmpHeader.sourceUuid, ptpClock->msgTmpHeader.sequenceId, ptpClock)) { /* * Incoming packets without hardware time stamp cannot be ignored outright because * a master might only be able to time stamp DelayReq packets; ignoring the Sync * packets from another, better clock would break the clock selection protocol. * Therefore set system time as fallback and decide below what to do. */ DBGV("*** message with no time stamp ***\n"); getTime(&time, ptpClock); badTime = TRUE; } } DBGV("%s 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", isEvent ? "event" : "control", 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, &ptpClock->runTimeOpts.inboundLatency); switch(ptpClock->msgTmpHeader.control) { case PTP_SYNC_MESSAGE: handleSync(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, badTime, isFromSelf, ptpClock); break; case PTP_FOLLOWUP_MESSAGE: handleFollowUp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, ptpClock); break; case PTP_DELAY_REQ_MESSAGE: handleDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, badTime, isFromSelf, ptpClock); break; case PTP_DELAY_RESP_MESSAGE: handleDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, ptpClock); break; case PTP_MANAGEMENT_MESSAGE: handleManagement(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, ptpClock); break; default: DBG("handle: unrecognized message\n"); break; } }
/* check and handle received messages */ static void handle(PtpClock *ptpClock) { int ret; Boolean isFromSelf; TimeInternal time = { 0, 0 }; if (FALSE == ptpClock->messageActivity) { ret = netSelect(&ptpClock->netPath, 0); if (ret < 0) { ERROR("handle: failed to poll sockets\n"); toState(ptpClock, PTP_FAULTY); return; } else if (!ret) { DBGVV("handle: nothing\n"); return; } } DBGVV("handle: something\n"); //msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader);//**********************************************/ ptpClock->msgIbufLength = netRecvEvent(&ptpClock->netPath, ptpClock->msgIbuf, &time); /* local time is not UTC, we can calculate UTC on demand, otherwise UTC time is not used */ /* time.seconds += ptpClock->timePropertiesDS.currentUtcOffset; */ if (ptpClock->msgIbufLength < 0) { ERROR("handle: failed to receive on the event socket\n"); toState(ptpClock, PTP_FAULTY); return; } else if (!ptpClock->msgIbufLength) { ptpClock->msgIbufLength = netRecvGeneral(&ptpClock->netPath, ptpClock->msgIbuf, &time); if (ptpClock->msgIbufLength < 0) { ERROR("handle: failed to receive on the general socket\n"); toState(ptpClock, PTP_FAULTY); return; } else if (!ptpClock->msgIbufLength) return; } ptpClock->messageActivity = TRUE; if (ptpClock->msgIbufLength < HEADER_LENGTH) { ERROR("handle: message shorter than header length\n"); toState(ptpClock, PTP_FAULTY); return; } msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader); if (ptpClock->msgTmpHeader.versionPTP != ptpClock->portDS.versionNumber) { DBGV("handle: ignore version %d message\n", ptpClock->msgTmpHeader.versionPTP); return; } if (ptpClock->msgTmpHeader.domainNumber != ptpClock->defaultDS.domainNumber) { DBGV("handle: ignore message from domainNumber %d\n", ptpClock->msgTmpHeader.domainNumber); return; } /*Spec 9.5.2.2*/ isFromSelf = isSamePortIdentity( &ptpClock->portDS.portIdentity, &ptpClock->msgTmpHeader.sourcePortIdentity); /* 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, &ptpClock->inboundLatency); switch (ptpClock->msgTmpHeader.messageType) { case ANNOUNCE: handleAnnounce(ptpClock, isFromSelf); break; case SYNC: handleSync(ptpClock, &time, isFromSelf); break; case FOLLOW_UP: handleFollowUp(ptpClock, isFromSelf); break; case DELAY_REQ: handleDelayReq(ptpClock, &time, isFromSelf); break; case PDELAY_REQ: handlePDelayReq(ptpClock, &time, isFromSelf); break; case DELAY_RESP: handleDelayResp(ptpClock, isFromSelf); break; case PDELAY_RESP: handlePDelayResp(ptpClock, &time, isFromSelf); break; case PDELAY_RESP_FOLLOW_UP: handlePDelayRespFollowUp(ptpClock, isFromSelf); break; case MANAGEMENT: handleManagement(ptpClock, isFromSelf); break; case SIGNALING: handleSignaling(ptpClock, isFromSelf); break; default: DBG("handle: unrecognized message %d\n", ptpClock->msgTmpHeader.messageType); break; } }
/* check and handle received messages */ void handle(RunTimeOpts *rtOpts, PtpClock *ptpClock) { int ret; ssize_t length; Boolean isFromSelf; TimeInternal time = { 0, 0 }; TimeInternal now = { 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) { #if 0 DBGV("handle: nothing\n"); #endif return; } /* else length > 0 */ } DBGV("handle: something\n"); length = netRecvEvent(ptpClock->msgIbuf, &time, &ptpClock->netPath); getTime(&now); DBG("Time is %ds %dns\n", now.seconds, now.nanoseconds); 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); DBG("event Receipt of Message\n type %d\n" " uuid %02x:%02x:%02x:%02x:%02x:%02x\n" " sequence %d\n time %us %dns\n", 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); 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; } }
/* 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"); /* TODO: this should be based on the select actual FDs (if(FD_ISSET(...)) */ 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, &time, &ptpClock->netPath); if (length < 0) { PERROR("failed to receive on the general socket"); toState(PTP_FAULTY, rtOpts, ptpClock); return; } else if (!length) return; } /* * make sure we use the TAI to UTC offset specified, if the master is sending the UTC_VALID bit * * * On the slave, all timestamps that we handle here have been collected by our local clock (loopback+kernel-level timestamp) * This includes delayReq just send, and delayResp, when it arrives. * * these are then adjusted to the same timebase of the Master (+34 leap seconds, as of 2011) * */ DBGV("__UTC_offset: %d %d \n", ptpClock->currentUtcOffsetValid, ptpClock->currentUtcOffset); if (ptpClock->currentUtcOffsetValid) { time.seconds += ptpClock->currentUtcOffset; } ptpClock->message_activity = TRUE; if (length < HEADER_LENGTH) { ERROR("message shorter than header length\n"); toState(PTP_FAULTY, rtOpts, ptpClock); return; } msgUnpackHeader(ptpClock->msgIbuf, &ptpClock->msgTmpHeader); if (ptpClock->msgTmpHeader.versionPTP != ptpClock->versionNumber) { DBG2("ignore version %d message\n", ptpClock->msgTmpHeader.versionPTP); return; } if(ptpClock->msgTmpHeader.domainNumber != ptpClock->domainNumber) { DBG2("ignore message from domainNumber %d\n", ptpClock->msgTmpHeader.domainNumber); return; } /*Spec 9.5.2.2*/ isFromSelf = (ptpClock->portIdentity.portNumber == ptpClock->msgTmpHeader.sourcePortIdentity.portNumber && !memcmp(ptpClock->msgTmpHeader.sourcePortIdentity.clockIdentity, ptpClock->portIdentity.clockIdentity, CLOCK_IDENTITY_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); #ifdef PTPD_DBG /* easy display of received messages */ char *st; switch(ptpClock->msgTmpHeader.messageType) { case ANNOUNCE: st = "Announce"; break; case SYNC: st = "Sync"; break; case FOLLOW_UP: st = "FollowUp"; break; case DELAY_REQ: st = "DelayReq"; break; case DELAY_RESP: st = "DelayResp"; break; default: st = "Unk"; break; } DBG(" ==> %s received\n", st); #endif /* * on the table below, note that only the event messsages are passed the local time, * (collected by us by loopback+kernel TS, and adjusted with UTC seconds * * (SYNC / DELAY_REQ / PDELAY_REQ / PDELAY_RESP) */ switch (ptpClock->msgTmpHeader.messageType) { case ANNOUNCE: handleAnnounce(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock); break; case SYNC: handleSync(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, isFromSelf, rtOpts, ptpClock); break; case FOLLOW_UP: handleFollowUp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock); break; case DELAY_REQ: handleDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, isFromSelf, rtOpts, ptpClock); break; case PDELAY_REQ: handlePDelayReq(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, &time, isFromSelf, rtOpts, ptpClock); break; case DELAY_RESP: handleDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock); break; case PDELAY_RESP: handlePDelayResp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, &time, length, isFromSelf, rtOpts, ptpClock); break; case PDELAY_RESP_FOLLOW_UP: handlePDelayRespFollowUp(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock); break; case MANAGEMENT: handleManagement(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock); break; case SIGNALING: handleSignaling(&ptpClock->msgTmpHeader, ptpClock->msgIbuf, length, isFromSelf, rtOpts, ptpClock); break; default: DBG("handle: unrecognized message\n"); break; } if (rtOpts->displayPackets) msgDump(ptpClock); }