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/bmc.c | 330 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 330 insertions(+) create mode 100644 third_party/ptpd-1.1.0/src/bmc.c (limited to 'third_party/ptpd-1.1.0/src/bmc.c') diff --git a/third_party/ptpd-1.1.0/src/bmc.c b/third_party/ptpd-1.1.0/src/bmc.c new file mode 100644 index 0000000..0b3ce74 --- /dev/null +++ b/third_party/ptpd-1.1.0/src/bmc.c @@ -0,0 +1,330 @@ +/** + * @file bmc.c + * @date Wed Jun 23 09:36:09 2010 + * + * @brief Best master clock selection code. + * + * The functions in this file are used by the daemon to select the + * best master clock from any number of possibilities. + */ + +#include "ptpd.h" + +void +initData(RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + DBG("initData\n"); + + if (rtOpts->slaveOnly) + rtOpts->clockStratum = 255; + + /* Port configuration data set */ + ptpClock->last_sync_event_sequence_number = 0; + ptpClock->last_general_event_sequence_number = 0; + ptpClock->port_id_field = 1; + ptpClock->burst_enabled = BURST_ENABLED; + + /* Default data set */ + ptpClock->clock_communication_technology = ptpClock->port_communication_technology; + memcpy(ptpClock->clock_uuid_field, ptpClock->port_uuid_field, PTP_UUID_LENGTH); + ptpClock->clock_port_id_field = 0; + ptpClock->clock_stratum = rtOpts->clockStratum; + memcpy(ptpClock->clock_identifier, rtOpts->clockIdentifier, PTP_CODE_STRING_LENGTH); + ptpClock->sync_interval = rtOpts->syncInterval; + + ptpClock->clock_variance = rtOpts->clockVariance; /* see spec 7.7 */ + ptpClock->clock_followup_capable = CLOCK_FOLLOWUP; + ptpClock->preferred = rtOpts->clockPreferred; + ptpClock->initializable = INITIALIZABLE; + ptpClock->external_timing = EXTERNAL_TIMING; + ptpClock->is_boundary_clock = BOUNDARY_CLOCK; + memcpy(ptpClock->subdomain_name, rtOpts->subdomainName, PTP_SUBDOMAIN_NAME_LENGTH); + ptpClock->number_ports = NUMBER_PORTS; + ptpClock->number_foreign_records = 0; + ptpClock->max_foreign_records = rtOpts->max_foreign_records; + + /* Global time properties data set */ + ptpClock->current_utc_offset = rtOpts->currentUtcOffset; + ptpClock->epoch_number = rtOpts->epochNumber; + + /* other stuff */ + ptpClock->random_seed = ptpClock->port_uuid_field[PTP_UUID_LENGTH - 1]; +} + +/* see spec table 18 */ +void +m1(PtpClock * ptpClock) +{ + /* Default data set */ + ptpClock->steps_removed = 0; + ptpClock->offset_from_master.seconds = 0; + ptpClock->offset_from_master.nanoseconds = 0; + ptpClock->one_way_delay.seconds = 0; + ptpClock->one_way_delay.nanoseconds = 0; + + /* Parent data set */ + ptpClock->parent_communication_technology = ptpClock->clock_communication_technology; + memcpy(ptpClock->parent_uuid, ptpClock->clock_uuid_field, PTP_UUID_LENGTH); + ptpClock->parent_port_id = ptpClock->clock_port_id_field; + ptpClock->parent_last_sync_sequence_number = 0; + ptpClock->parent_followup_capable = ptpClock->clock_followup_capable; + ptpClock->parent_external_timing = ptpClock->external_timing; + ptpClock->parent_variance = ptpClock->clock_variance; + ptpClock->grandmaster_communication_technology = ptpClock->clock_communication_technology; + memcpy(ptpClock->grandmaster_uuid_field, ptpClock->clock_uuid_field, PTP_UUID_LENGTH); + ptpClock->grandmaster_port_id_field = ptpClock->clock_port_id_field; + ptpClock->grandmaster_stratum = ptpClock->clock_stratum; + memcpy(ptpClock->grandmaster_identifier, ptpClock->clock_identifier, PTP_CODE_STRING_LENGTH); + ptpClock->grandmaster_variance = ptpClock->clock_variance; + ptpClock->grandmaster_preferred = ptpClock->preferred; + ptpClock->grandmaster_is_boundary_clock = ptpClock->is_boundary_clock; + ptpClock->grandmaster_sequence_number = ptpClock->last_sync_event_sequence_number; +} + +/* see spec table 21 */ +void +s1(MsgHeader * header, MsgSync * sync, PtpClock * ptpClock) +{ + /* Current data set */ + ptpClock->steps_removed = sync->localStepsRemoved + 1; + + /* Parent data set */ + ptpClock->parent_communication_technology = header->sourceCommunicationTechnology; + memcpy(ptpClock->parent_uuid, header->sourceUuid, PTP_UUID_LENGTH); + ptpClock->parent_port_id = header->sourcePortId; + ptpClock->parent_last_sync_sequence_number = header->sequenceId; + ptpClock->parent_followup_capable = getFlag(header->flags, PTP_ASSIST); + ptpClock->parent_external_timing = getFlag(header->flags, PTP_EXT_SYNC); + ptpClock->parent_variance = sync->localClockVariance; + ptpClock->grandmaster_communication_technology = sync->grandmasterCommunicationTechnology; + memcpy(ptpClock->grandmaster_uuid_field, sync->grandmasterClockUuid, PTP_UUID_LENGTH); + ptpClock->grandmaster_port_id_field = sync->grandmasterPortId; + ptpClock->grandmaster_stratum = sync->grandmasterClockStratum; + memcpy(ptpClock->grandmaster_identifier, sync->grandmasterClockIdentifier, PTP_CODE_STRING_LENGTH); + ptpClock->grandmaster_variance = sync->grandmasterClockVariance; + ptpClock->grandmaster_preferred = sync->grandmasterPreferred; + ptpClock->grandmaster_is_boundary_clock = sync->grandmasterIsBoundaryClock; + ptpClock->grandmaster_sequence_number = sync->grandmasterSequenceId; + + /* Global time properties data set */ + ptpClock->current_utc_offset = sync->currentUTCOffset; + ptpClock->leap_59 = getFlag(header->flags, PTP_LI_59); + ptpClock->leap_61 = getFlag(header->flags, PTP_LI_61); + ptpClock->epoch_number = sync->epochNumber; +} + +void +copyD0(MsgHeader * header, MsgSync * sync, PtpClock * ptpClock) +{ + sync->grandmasterCommunicationTechnology = ptpClock->clock_communication_technology; + memcpy(sync->grandmasterClockUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH); + sync->grandmasterPortId = ptpClock->port_id_field; + sync->grandmasterClockStratum = ptpClock->clock_stratum; + memcpy(sync->grandmasterClockIdentifier, ptpClock->clock_identifier, PTP_CODE_STRING_LENGTH); + sync->grandmasterClockVariance = ptpClock->clock_variance; + sync->grandmasterIsBoundaryClock = ptpClock->is_boundary_clock; + sync->grandmasterPreferred = ptpClock->preferred; + sync->localStepsRemoved = ptpClock->steps_removed; + header->sourceCommunicationTechnology = ptpClock->clock_communication_technology; + memcpy(header->sourceUuid, ptpClock->port_uuid_field, PTP_UUID_LENGTH); + header->sourcePortId = ptpClock->port_id_field; + sync->grandmasterSequenceId = ptpClock->grandmaster_sequence_number; + header->sequenceId = ptpClock->grandmaster_sequence_number; +} + +int +getIdentifierOrder(Octet identifier[PTP_CODE_STRING_LENGTH]) +{ + if (!memcmp(identifier, IDENTIFIER_ATOM, PTP_CODE_STRING_LENGTH)) + return 1; + else if (!memcmp(identifier, IDENTIFIER_GPS, PTP_CODE_STRING_LENGTH)) + return 1; + else if (!memcmp(identifier, IDENTIFIER_NTP, PTP_CODE_STRING_LENGTH)) + return 2; + else if (!memcmp(identifier, IDENTIFIER_HAND, PTP_CODE_STRING_LENGTH)) + return 3; + else if (!memcmp(identifier, IDENTIFIER_INIT, PTP_CODE_STRING_LENGTH)) + return 4; + else if (!memcmp(identifier, IDENTIFIER_DFLT, PTP_CODE_STRING_LENGTH)) + return 5; + + return 6; +} + +/* return similar to memcmp()s + note: communicationTechnology can be ignored because + if they differed they would not have made it here */ +Integer8 +bmcDataSetComparison(MsgHeader * headerA, MsgSync * syncA, + MsgHeader * headerB, MsgSync * syncB, PtpClock * ptpClock) +{ + DBGV("bmcDataSetComparison: start\n"); + if (!(syncA->grandmasterPortId == syncB->grandmasterPortId + && !memcmp(syncA->grandmasterClockUuid, syncB->grandmasterClockUuid, PTP_UUID_LENGTH))) { + if (syncA->grandmasterClockStratum < syncB->grandmasterClockStratum) + goto A; + else if (syncA->grandmasterClockStratum > syncB->grandmasterClockStratum) + goto B; + + /* grandmasterClockStratums same */ + if (getIdentifierOrder(syncA->grandmasterClockIdentifier) < getIdentifierOrder(syncB->grandmasterClockIdentifier)) + goto A; + if (getIdentifierOrder(syncA->grandmasterClockIdentifier) > getIdentifierOrder(syncB->grandmasterClockIdentifier)) + goto B; + + /* grandmasterClockIdentifiers same */ + if (syncA->grandmasterClockStratum > 2) { + if (syncA->grandmasterClockVariance > syncB->grandmasterClockVariance + PTP_LOG_VARIANCE_THRESHOLD + || syncA->grandmasterClockVariance < syncB->grandmasterClockVariance - PTP_LOG_VARIANCE_THRESHOLD) { + /* grandmasterClockVariances are not similar */ + if (syncA->grandmasterClockVariance < syncB->grandmasterClockVariance) + goto A; + else + goto B; + } + /* grandmasterClockVariances are similar */ + if (!syncA->grandmasterIsBoundaryClock != !syncB->grandmasterIsBoundaryClock) { /* XOR */ + if (syncA->grandmasterIsBoundaryClock) + goto A; + else + goto B; + } + /* neither is grandmasterIsBoundaryClock */ + if (memcmp(syncA->grandmasterClockUuid, syncB->grandmasterClockUuid, PTP_UUID_LENGTH) < 0) + goto A; + else + goto B; + } + /* syncA->grandmasterClockStratum <= 2 */ + if (!syncA->grandmasterPreferred != !syncB->grandmasterPreferred) { /* XOR */ + if (syncA->grandmasterPreferred) + return 1; /* A1 */ + else + return -1; /* B1 */ + } + /* neither or both grandmasterPreferred */ + } + DBGV("bmcDataSetComparison: X\n"); + if (syncA->localStepsRemoved > syncB->localStepsRemoved + 1 + || syncA->localStepsRemoved < syncB->localStepsRemoved - 1) { + /* localStepsRemoved not within 1 */ + if (syncA->localStepsRemoved < syncB->localStepsRemoved) + return 1; /* A1 */ + else + return -1; /* B1 */ + } + /* localStepsRemoved within 1 */ + if (syncA->localStepsRemoved < syncB->localStepsRemoved) { + DBGV("bmcDataSetComparison: A3\n"); + if (memcmp(ptpClock->port_uuid_field, headerB->sourceUuid, PTP_UUID_LENGTH) < 0) + return 1; /* A1 */ + else if (memcmp(ptpClock->port_uuid_field, headerB->sourceUuid, PTP_UUID_LENGTH) > 0) + return 2; /* A2 */ + + /* this port_uuid_field same as headerB->sourceUuid */ + if (ptpClock->port_id_field < headerB->sourcePortId) + return 1; /* A1 */ + else if (ptpClock->port_id_field > headerB->sourcePortId) + return 2; /* A2 */ + + /* this port_id_field same as headerB->sourcePortId */ + return 0; /* same */ + } + if (syncA->localStepsRemoved > syncB->localStepsRemoved) { + DBGV("bmcDataSetComparison: B3\n"); + if (memcmp(ptpClock->port_uuid_field, headerA->sourceUuid, PTP_UUID_LENGTH) < 0) + return -1; /* B1 */ + else if (memcmp(ptpClock->port_uuid_field, headerB->sourceUuid, PTP_UUID_LENGTH) > 0) + return -2; /* B2 */ + + /* this port_uuid_field same as headerA->sourceUuid */ + if (ptpClock->port_id_field < headerA->sourcePortId) + return -1; /* B1 */ + else if (ptpClock->port_id_field > headerA->sourcePortId) + return -2; /* B2 */ + + /* this port_id_field same as headerA->sourcePortId */ + return 0; /* same */ + } + /* localStepsRemoved same */ + if (memcmp(headerA->sourceUuid, headerB->sourceUuid, PTP_UUID_LENGTH) < 0) + return 2; /* A2 */ + else if (memcmp(headerA->sourceUuid, headerB->sourceUuid, PTP_UUID_LENGTH) > 0) + return -2; /* B2 */ + + /* sourceUuid same */ + DBGV("bmcDataSetComparison: Z\n"); + if (syncA->grandmasterSequenceId > syncB->grandmasterSequenceId) + return 3; + else if (syncA->grandmasterSequenceId < syncB->grandmasterSequenceId) + return -3; + + /* grandmasterSequenceId same */ + if (headerA->sequenceId > headerB->sequenceId) + return 3; + else if (headerA->sequenceId < headerB->sequenceId) + return -3; + + /* sequenceId same */ + return 0; /* same */ + + /* oh no, a goto label! the horror! */ +A: + if (!syncA->grandmasterPreferred && syncB->grandmasterPreferred) + return -1; /* B1 */ + else + return 1; /* A1 */ +B: + if (syncA->grandmasterPreferred && !syncB->grandmasterPreferred) + return 1; /* A1 */ + else + return -1; /* B1 */ +} + +UInteger8 +bmcStateDecision(MsgHeader * header, MsgSync * sync, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + if (rtOpts->slaveOnly) { + s1(header, sync, ptpClock); + return PTP_SLAVE; + } + copyD0(&ptpClock->msgTmpHeader, &ptpClock->msgTmp.sync, ptpClock); + + if (ptpClock->msgTmp.sync.grandmasterClockStratum < 3) { + if (bmcDataSetComparison(&ptpClock->msgTmpHeader, &ptpClock->msgTmp.sync, header, sync, ptpClock) > 0) { + m1(ptpClock); + return PTP_MASTER; + } + s1(header, sync, ptpClock); + return PTP_PASSIVE; + } else if (bmcDataSetComparison(&ptpClock->msgTmpHeader, &ptpClock->msgTmp.sync, header, sync, ptpClock) > 0 + && ptpClock->msgTmp.sync.grandmasterClockStratum != 255) { + m1(ptpClock); + return PTP_MASTER; + } else { + s1(header, sync, ptpClock); + return PTP_SLAVE; + } +} + +UInteger8 +bmc(ForeignMasterRecord * foreign, RunTimeOpts * rtOpts, PtpClock * ptpClock) +{ + Integer16 i, best; + + if (!ptpClock->number_foreign_records) { + if (ptpClock->port_state == PTP_MASTER) + m1(ptpClock); + return ptpClock->port_state; /* no change */ + } + for (i = 1, best = 0; i < ptpClock->number_foreign_records; ++i) { + if (bmcDataSetComparison(&foreign[i].header, &foreign[i].sync, + &foreign[best].header, &foreign[best].sync, ptpClock) > 0) + best = i; + } + + DBGV("bmc: best record %d\n", best); + ptpClock->foreign_record_best = best; + + return bmcStateDecision(&foreign[best].header, &foreign[best].sync, rtOpts, ptpClock); +} -- cgit v1.3.1