ptp_util.c

Go to the documentation of this file.

/*
 * ptp_util.c
 *
 *  Date: 19/06/2014
 *  Author: Fernando Biazi Nascimento
 *  Copyright © 2014 Fernando Biazi Nascimento. All rights reserved.
 *
 *  License of use and copy on file license.txt
 *
 */

#include <stdlib.h>

/*
 * ======== Includes ========
 */

#include "ptp_util.h"
#include "ptp.h"
#include "ptp_const.h"
#include "ptp_cfg.h"
#include "ptp_types.h"
#include "ptp_messages.h"
#include "clock.h"
#include "nwk.h"

/*
 * ======== Constants ========
 */

/*
 * ======== Macros ========
 */

/*
 * ======== Types ========
 */

/*
 * ======== Global Variables ========
 */

/*
 * ======== Local Variables ========
 */

ptp_recommendedState ptp_portRecommendedState[PTP_NUMBERPORTS];
PTP_announceDS ptp_announce[PTP_NUMBERPORTS];

/*
 * ======== Local Functions ========
 */

void ptp_bmc();
void ptp_portUpdateDS();
void ptp_portUpdateStatus();

/*
 * ================
 */

ptp_clockIdentity * ptp_getClockIdentity() {
//  // old code used to write return value on a variable, It didn't return a pointer
//  ptp_integer64 tmp;
//  tmp = PTP_OUI; // Get OUI, 24 bits
//  tmp <<= 40; // put OUI on place
//  tmp |= PTP_EXTENSION_IDENTIFIER; // include extension identifier (40 bits)
//  memcpy(result, &tmp, 8);
  return (ptp_clockIdentity *) nwk_getPIeeeAddress();
}

void ptp_getCurrentTime(PTP_TimeStamp *dest) {
  clock_getTime((void *) dest);
}

void ptp_adjTime(PTP_TimeStamp *t1, PTP_TimeStamp *t2, ptp_uinteger8 portIndex) {
  static ptp_uinteger8 adjustTryAgain = 0;
  ptp_integer64 offsetFromMaster;

  // If apparent offset is greater than 30s, just set the clock to the received
  // time. Moreover, there would be division to adjust the clock above 32s, an
  // expensive task. Precise adjustments will be done later.
  offsetFromMaster = t2->secondsField.secondsHigh;
  offsetFromMaster -= t1->secondsField.secondsHigh;
  offsetFromMaster <<= 32;
  offsetFromMaster += t2->secondsField.secondsLow;
  offsetFromMaster -= t1->secondsField.secondsLow; // offset is seconds

//    if (t2->secondsField.secondsHigh != t1.secondsField.secondsHigh) {
//      offsetFromMaster = t2->secondsField.secondsHigh
//          - t1.secondsField.secondsHigh;
//      offsetFromMaster <<= 32;
//      offsetFromMaster += offsetFromMasterLow;
//    } else {
//      offsetFromMaster = offsetFromMasterLow;
//    }

  if ((offsetFromMaster < -30) || (offsetFromMaster > 30)) {
    adjustTryAgain = 0; // Next adjust will be done even if apparent offset > 4s.
    // set time
    clock_setTime((void *) t1); // Expects PTP_TimeStamp and clock_timeStamp to be bit compatible
    // get time stamp (t2) again, that will be invalid after setting the clock.
    ptp_getCurrentTime(t2);
  } else {
    // time is within ~30s.

    // offset is calculated in ns.
    offsetFromMaster = clock_timeDiff((void*) t1, (void *) t2); // Expects PTP_TimeStamp and clock_timeStamp to be bit compatible
    // offsetFromMaster doesn't count on ptp_meanPathDelay at this point, to save processing

    // If offset is larger than 4s, do not adjust the clock
    // but if it repeat 3 times, adjust it anyway.
    if (adjustTryAgain
        && ((offsetFromMaster > 2000000000)
            || (-offsetFromMaster > 2000000000))) {
      adjustTryAgain--;
      ptp_lastAdjust = 0;
      ptp_getCurrentTime(&ptp_lastAdjustTime);
    } else {
      adjustTryAgain = 2;
      ptp_meanDelayAtLastAdjust = ptp_meanPathDelay[portIndex] >> 16;
      offsetFromMaster -= ptp_meanDelayAtLastAdjust;
      // to send to adjust, offset must have inverted signal
      offsetFromMaster = -offsetFromMaster;
      clock_adjTime(&offsetFromMaster);
      ptp_getCurrentTime(&ptp_lastAdjustTime);
      ptp_lastAdjust = offsetFromMaster;
      // adjust time stamp t2 to the same offset, as it would become invalid
      // after adjusting the clock
      clock_adjTimeValues(&offsetFromMaster, (void *) t2); // Expects PTP_TimeStamp and clock_timeStamp to be bit compatible
    }
  }
}

ptp_integer64 ptp_timeDiff(PTP_TimeStamp *t1, PTP_TimeStamp *t2) {
  return clock_timeDiff((void *) t1, (void *) t2) << 16;
}

ptp_integer8 ptp_clockIdentityCompare(ptp_clockIdentity A, ptp_clockIdentity B) {
  ptp_uinteger8 i = 8; // little endian, last byte is most significant, compare it first
  while (i) {
    i--;
    if (A[i] > B[i]) {
      return 1;
    } else if (A[i] < B[i]) {
      return -1;
    }
  }
  return 0;
}

void ptp_portsSetListening() {
  ptp_uinteger8 portIndex;

  if (nwk_getDeviceType() == NWK_DEVTYPE_ENDDEVICE)
    ptpClock.defaultDS.numberPorts = 1;
  portIndex = ptpClock.defaultDS.numberPorts;
  while (portIndex) {
    portIndex--;
    ptp_copyOctets(8, PTP_ZEROSEQ64,
        ptp_foreignMaster[portIndex][0].clockIdentity);
    ptp_copyOctets(8, PTP_ZEROSEQ64,
        ptp_announce[portIndex].grandMasterIdentity);
    ptpClock.portDS[portIndex].portState = PTP_PS_LISTENING;
  }
}

void ptp_state_decision_event() {

  // run BMC
  ptp_bmc();

  // update data sets - all ports
  ptp_portUpdateDS();

  // state changes - all ports
  ptp_portUpdateStatus();

}

void ptp_bmc() {
  ptp_uinteger8 portNumber;
  ptp_uinteger8 portIndex;
  ptp_uinteger8 iEbest;
  ptp_boolean Dbetter;
  ptp_boolean byTopology;

  // Each port "r" compute Erbest. IEC 61588-2009 / IEEE 1588-2008 9.3.2.2 b)
  // On this program, each port stores only one message.

  // Choose Ebest from all Erbest. IEC 61588-2009 / IEEE 1588-2008 9.3.2.2 c)
  iEbest = 0; // if no announces are found, iEbest will remain 0
  portNumber = ptpClock.defaultDS.numberPorts;
  while (portNumber) {
    if (nwk_getNeighborAddress(portNumber) != 0xFFFF)
      ptp_dataSetComparisonEE(&iEbest, &byTopology,
          &(ptp_announce[iEbest - 1]),
          &(ptp_announce[portNumber - 1]), iEbest, portNumber);
    portNumber--;
  };
  if (iEbest != 0)
    if (ptp_clockIdentityCompare(
        ptp_announce[iEbest - 1u].grandMasterIdentity, PTP_ZEROSEQ64)
        == 0)
      iEbest = 0;

  // state decision algorithm 9.3.3
  portNumber = ptpClock.defaultDS.numberPorts;
  while (portNumber) {
    portIndex = portNumber - 1;
    if ((ptp_clockIdentityCompare(
        ptp_announce[portIndex].grandMasterIdentity, PTP_ZEROSEQ64) == 0)
        && (ptpClock.portDS[portNumber].portState == PTP_BMC_LISTENING))
      ptp_portRecommendedState[portNumber] = PTP_BMC_LISTENING;
    else {
      Dbetter = PTP_TRUE;
      ptp_dataSetComparisonDE(&Dbetter, &ptpClock,
          &(ptp_announce[portIndex]), portNumber);
      if (ptpClock.defaultDS.clockQuality.clockClass < 128) {
        if (Dbetter)
          ptp_portRecommendedState[portIndex] = PTP_BMC_M1;
        else
          ptp_portRecommendedState[portIndex] = PTP_BMC_P1;
      } else {
        if (Dbetter)
          ptp_portRecommendedState[portIndex] = PTP_BMC_M2;
        else if (portNumber == iEbest)
          ptp_portRecommendedState[portIndex] = PTP_BMC_S1;
        else {
          ptp_dataSetComparisonEE(&iEbest, &byTopology,
              &(ptp_announce[iEbest - 1]),
              &(ptp_announce[portIndex]), iEbest, portNumber);
          if (byTopology)
            ptp_portRecommendedState[portIndex] = PTP_BMC_P2;
          else
            ptp_portRecommendedState[portIndex] = PTP_BMC_M3;
        }
      }
    }
    portNumber--;
  }
}

void ptp_portUpdateDS() {
  ptp_uinteger8 portIndex;

  portIndex = ptpClock.defaultDS.numberPorts;
  while (portIndex) {
    portIndex--;
    if (nwk_getNeighborAddress(portIndex + 1) != 0xFFFF) {
      switch (ptp_portRecommendedState[portIndex]) {
      case PTP_BMC_M1:
      case PTP_BMC_M2:
        // table 13
        ptpClock.currentDS.stepsRemoved = 0;
        ptpClock.currentDS.offsetFromMaster.scalatedNanoseconds = 0;
        ptpClock.currentDS.meanPathDelay.scalatedNanoseconds = 0;
        ptpClock.parentDS.parentPortIdentity.clockIdentity =
            ptpClock.defaultDS.clockIdentity;
        ptpClock.parentDS.parentPortIdentity.portNumber = 0;
        ptpClock.parentDS.grandMasterIdentity =
            ptpClock.defaultDS.clockIdentity;
        ptpClock.parentDS.grandMasterclockQuality.clockAccuracy =
            ptpClock.defaultDS.clockQuality.clockAccuracy;
        ptpClock.parentDS.grandMasterclockQuality.clockClass =
            ptpClock.defaultDS.clockQuality.clockClass;
        ptpClock.parentDS.grandMasterclockQuality.offsetScaledLogVariance =
            ptpClock.defaultDS.clockQuality.offsetScaledLogVariance;
        ptpClock.parentDS.grandMasterPriority1 =
            ptpClock.defaultDS.priority1;
        ptpClock.parentDS.grandMasterPriority2 =
            ptpClock.defaultDS.priority2;
        ptpClock.timePropertiesDS.currentUtcOffset =
            PTP_DESIGNTIME_UTCOFFSET; // 9.3.5, 9.4, 7.2.3
        ptpClock.timePropertiesDS.currentUtcOffsetValid = PTP_FALSE; // 9.3.5, 9.4
        ptpClock.timePropertiesDS.leap59 = PTP_FALSE; // 9.3.5, 9.4
        ptpClock.timePropertiesDS.leap61 = PTP_FALSE; // 9.3.5, 9.4
        ptpClock.timePropertiesDS.timeTraceable = PTP_FALSE; // 9.3.5, 9.4
        ptpClock.timePropertiesDS.frequencyTraceable = PTP_FALSE; // 9.3.5, 9.4
        ptpClock.timePropertiesDS.ptpTimescale = PTP_FALSE; // 9.3.5, 9.4
        // ptpClock.timePropertiesDS.timeSource = PTP_TS_INTERNAL_OSCILLATOR;
        // 9.3.5, 9.4, Could be gathered from  device time source.
        // this is set on ptp_clock_initialize, ptp_proccess, ptp_setNetTime, or when state is PTP_BMC_S1
        portIndex = 0; // to interrupt processing, as all ports will result in this same code being run.
        break;
      case PTP_BMC_M3:
        // table 14

        break;
      case PTP_BMC_P1:
      case PTP_BMC_P2:
        // table 15

        break;
      case PTP_BMC_S1:
        // table 16
        ptpClock.currentDS.stepsRemoved =
            ptp_announce[portIndex].stepsRemoved + 1;
        ptpClock.parentDS.parentPortIdentity.clockIdentity =
            &(ptp_foreignMaster[portIndex][0].clockIdentity);
        ptpClock.parentDS.parentPortIdentity.portNumber =
            nwk_getNeighborForeignPort(portIndex + 1);
        ptpClock.parentDS.grandMasterIdentity =
            &(ptp_announce[portIndex].grandMasterIdentity);
        ptpClock.parentDS.grandMasterclockQuality.clockAccuracy =
            ptp_announce[portIndex].grandMasterClockQuality.clockAccuracy;
        ptpClock.parentDS.grandMasterclockQuality.clockClass =
            ptp_announce[portIndex].grandMasterClockQuality.clockClass;
        ptpClock.parentDS.grandMasterclockQuality.offsetScaledLogVariance =
            ptp_announce[portIndex].grandMasterClockQuality.offsetScaledLogVariance;
        ptpClock.parentDS.grandMasterPriority1 =
            ptp_announce[portIndex].grandMasterPriority1;
        ptpClock.parentDS.grandMasterPriority2 =
            ptp_announce[portIndex].grandMasterPriority1;
        ptpClock.timePropertiesDS.currentUtcOffset =
            ptp_announce[portIndex].currentUtcOffset;
        ptpClock.timePropertiesDS.currentUtcOffsetValid =
            ptp_announce[portIndex].currentUtcOffsetValid;
        ptpClock.timePropertiesDS.leap59 =
            ptp_announce[portIndex].leap59;
        ptpClock.timePropertiesDS.leap61 =
            ptp_announce[portIndex].leap61;
        ptpClock.timePropertiesDS.timeTraceable =
            ptp_announce[portIndex].timeTraceable;
        ptpClock.timePropertiesDS.frequencyTraceable =
            ptp_announce[portIndex].frequencyTraceable;
        ptpClock.timePropertiesDS.ptpTimescale =
            ptp_announce[portIndex].ptpTimescale;
        ptpClock.timePropertiesDS.timeSource =
            ptp_announce[portIndex].timeSource;
        break;
      default:
        break;
      }
    }
  }
}

void ptp_portUpdateStatus() {
  ptp_uinteger8 i;
  i = ptpClock.defaultDS.numberPorts;
  while (i) {
    i--;
    switch (ptp_portRecommendedState[i]) {
    case PTP_BMC_M1:
    case PTP_BMC_M2:
    case PTP_BMC_M3:
      if (ptpClock.portDS[i].portState == PTP_PS_LISTENING
          || ptpClock.portDS[i].portState == PTP_PS_UNCALIBRATED
          || ptpClock.portDS[i].portState == PTP_PS_SLAVE
          || ptpClock.portDS[i].portState == PTP_PS_PASSIVE) {
        ptpClock.portDS[i].portState = PTP_PS_PRE_MASTER;
      }
      break;
    case PTP_BMC_S1:
      if (ptpClock.portDS[i].portState == PTP_PS_LISTENING
          || ptpClock.portDS[i].portState == PTP_PS_UNCALIBRATED
          || ptpClock.portDS[i].portState == PTP_PS_PRE_MASTER
          || ptpClock.portDS[i].portState == PTP_PS_MASTER
          || ptpClock.portDS[i].portState == PTP_PS_PASSIVE) {
        ptpClock.portDS[i].portState = PTP_PS_UNCALIBRATED;
      }
      break;
    case PTP_BMC_P1:
    case PTP_BMC_P2:
      if (ptpClock.portDS[i].portState == PTP_PS_LISTENING
          || ptpClock.portDS[i].portState == PTP_PS_UNCALIBRATED
          || ptpClock.portDS[i].portState == PTP_PS_SLAVE
          || ptpClock.portDS[i].portState == PTP_PS_PRE_MASTER
          || ptpClock.portDS[i].portState == PTP_PS_MASTER) {
        ptpClock.portDS[i].portState = PTP_PS_PASSIVE;
      }
      break;
    default:
      break;
    }
  }
}

ptp_integer8 ptp_dataSetComparisonEE(ptp_uinteger8 *iBetter,
    ptp_boolean *byTopology, PTP_announceDS *E1, PTP_announceDS *E2,
    ptp_uinteger8 i1, ptp_uinteger8 i2) {
  *byTopology = PTP_FALSE;

  // check for empty
  if (i1)
    if (ptp_clockIdentityCompare(E1->grandMasterIdentity, PTP_ZEROSEQ64)
        == 0)
      i1 = 0;
  if (i2)
    if (ptp_clockIdentityCompare(E2->grandMasterIdentity, PTP_ZEROSEQ64)
        == 0)
      i2 = 0;
  if ((i1 == 0) && (i2 == 0))
    return 3; // not on standard
  else if (i1 == 0) {
    *iBetter = i2;
    return 0;
  } else if (i2 == 0) {
    *iBetter = i1;
    return 0;
  }

  // check for same grand master
  if (ptp_clockIdentityCompare(E1->grandMasterIdentity,
      E2->grandMasterIdentity) == 0) {
    // steps removed
    if (E1->stepsRemoved > (E2->stepsRemoved + 1)) {
      *iBetter = i2;
      return 0;
    } else if ((E1->stepsRemoved + 1) < E2->stepsRemoved) {
      *iBetter = i1;
      return 0;
    }
    // E1->stepsRemoved within 1 of E2->stepsRemoved
    if (E1->stepsRemoved > E2->stepsRemoved) {
      switch (ptp_clockIdentityCompare(
          *(ptpClock.defaultDS.clockIdentity),
          ptp_foreignMaster[i1 - 1u][0].clockIdentity)) {
      case -1:
        // if receiver Identity of E1 < sender Identity of E1, E2 is better
        *iBetter = i2;
        return 0;
      case 1:
        // if receiver Identity of E1 > sender Identity of E1, E2 is better by topology
        *iBetter = i2;
        *byTopology = PTP_TRUE;
        return 0;
      default:
        break;
      }
      // if receiver Identity of E1 = sender Identity of E1, error 1
      return 1;
    } else if (E1->stepsRemoved < E2->stepsRemoved) {
      switch (ptp_clockIdentityCompare(
          *(ptpClock.defaultDS.clockIdentity),
          ptp_foreignMaster[i2 - 1u][0].clockIdentity)) {
      case -1:
        // if receiver Identity of E2 < sender Identity of E2, E1 is better
        *iBetter = i1;
        return 0;
      case 1:
        // if receiver Identity of E2 > sender Identity of E2, E1 is better by topology
        *iBetter = i1;
        *byTopology = PTP_TRUE;
        return 0;
      default:
        break;
      }
      // if receiver Identity of E2 = sender Identity of E2, error 1
      return -1;
    }
    // Identities of senders
    switch (ptp_clockIdentityCompare(
        ptp_foreignMaster[i1 - 1u][0].clockIdentity,
        ptp_foreignMaster[i2 - 1u][0].clockIdentity)) {
    case 1:
      *iBetter = i2;
      *byTopology = PTP_TRUE;
      return 0;
    case -1:
      *iBetter = i1;
      *byTopology = PTP_TRUE;
      return 0;
    default:
      break;
    }
    // port numbers of receivers
    if (i1 > i2) {
      // E2 better by topology
      *iBetter = i2;
      *byTopology = PTP_TRUE;
      return 0;
    } else if (i1 < i2) {
      // E1 better by topology
      *iBetter = i1;
      *byTopology = PTP_TRUE;
      return 0;
    }
  } else {
    // Compare grand master clocks.
    // priority1
    if (E1->grandMasterPriority1 > E2->grandMasterPriority1) {
      *iBetter = i2;
      return 0;
    } else if (E1->grandMasterPriority1 < E2->grandMasterPriority1) {
      *iBetter = i1;
      return 0;
    }
    // class
    if (E1->grandMasterClockQuality.clockClass
        > E2->grandMasterClockQuality.clockClass) {
      *iBetter = i2;
      return 0;
    } else if (E1->grandMasterClockQuality.clockClass
        < E2->grandMasterClockQuality.clockClass) {
      *iBetter = i1;
      return 0;
    }
    // accuracy
    if (E1->grandMasterClockQuality.clockAccuracy
        > E2->grandMasterClockQuality.clockAccuracy) {
      *iBetter = i2;
      return 0;
    } else if (E1->grandMasterClockQuality.clockAccuracy
        < E2->grandMasterClockQuality.clockAccuracy) {
      *iBetter = i1;
      return 0;
    }
    // offsetScaledLogVariance
    if (E1->grandMasterClockQuality.offsetScaledLogVariance
        > E2->grandMasterClockQuality.offsetScaledLogVariance) {
      *iBetter = i2;
      return 0;
    } else if (E1->grandMasterClockQuality.offsetScaledLogVariance
        < E2->grandMasterClockQuality.offsetScaledLogVariance) {
      *iBetter = i1;
      return 0;
    }
    // priority2
    if (E1->grandMasterPriority2 > E2->grandMasterPriority2) {
      *iBetter = i2;
      return 0;
    } else if (E1->grandMasterPriority2 < E2->grandMasterPriority2) {
      *iBetter = i1;
      return 0;
    }
    // Identity
    switch (ptp_clockIdentityCompare(E1->grandMasterIdentity,
        E2->grandMasterIdentity)) {
    case 1:
      *iBetter = i2;
      return 0;
    case -1:
      *iBetter = i1;
      return 0;
    default:
      break;
    }
  }
  return 2;
}

ptp_integer8 ptp_dataSetComparisonDE(ptp_boolean *Dbetter, ptp_clock *D,
    PTP_announceDS *E, ptp_uinteger8 portNumber) {
  ptp_uinteger8 portIndex;

  // check for no address (empty set)
  if (portNumber) {
    if (ptp_clockIdentityCompare(E->grandMasterIdentity, PTP_ZEROSEQ64)
        == 0)
      portNumber = 0;
  }
  if (!portNumber) {
    *Dbetter = PTP_TRUE;
    return 0;
  }

  if (ptp_clockIdentityCompare(*(D->defaultDS.clockIdentity),
      E->grandMasterIdentity) == 0) {
//    steps removed
//    this will never happen
//    if (0 > (E->stepsRemoved + 1)) {
//      *Dbetter = PTP_FALSE;
//      return 0;
//    } else
    if (1 < E->stepsRemoved) {
      *Dbetter = PTP_TRUE;
      return 0;
    }
    portIndex = portNumber - 1;
//     D->stepsRemoved within 1 of E->stepsRemoved
//     stepsRemoved is an unsigned integer, (0 > E->stepsRemoved) is always false.
//    if (0 > E->stepsRemoved) {
//     if receiver Identity of D < sender Identity of D, E is better
//     This should not apply because D is the data sets of the clock,
//     there is no sender or receiver.
//
//     if Identity of receiver of D > Identity of sender of D, E is better by topology
//     This should not apply because D is the data sets of the clock,
//     there is no sender or receiver.
//
//     if Identity of receiver of E1 = Identity of sender of E1, error 1
//      *Dbetter = PTP_FALSE;
//      return 0;
//
//    } else
    // (0 < E->stepsRemoved) becomes (0 != E->stepsRemoved)
    if (0 != E->stepsRemoved) {
      switch (ptp_clockIdentityCompare(*(D->defaultDS.clockIdentity),
          ptp_foreignMaster[portIndex][0].clockIdentity)) {
      case -1:
        // if Identity of receiver of E < Identity of sender of E, D is better
        *Dbetter = PTP_TRUE;
        return 0;
      case 1:
        // if Identity of receiver of E > Identity of sender of E, D is better by topology
        *Dbetter = PTP_TRUE;
        return 0;
      default:
        break;
      }
      // if receiver Identity of E = sender Identity of E, error 1
      return 1;
    }
    // Identities of senders
    switch (ptp_clockIdentityCompare(*(D->defaultDS.clockIdentity),
        ptp_foreignMaster[portIndex][0].clockIdentity)) {
    case 1:
      // D better by topology
      *Dbetter = PTP_FALSE;
      return 0;
    case -1:
      // E better by topology
      *Dbetter = PTP_TRUE;
      return 0;
    default:
      break;
    }
    // port numbers of receivers
    // D0 port number is always 0.
    // D0 port number < E port Number : D0 is better by topology
    *Dbetter = PTP_TRUE;
    return 0;
  } else {
    // Compare grand master clocks.
    // priority1
    if (D->defaultDS.priority1 > E->grandMasterPriority1) {
      *Dbetter = PTP_FALSE;
      return 0;
    } else if (D->defaultDS.priority1 < E->grandMasterPriority1) {
      *Dbetter = PTP_TRUE;
      return 0;
    }
    // class
    if (D->defaultDS.clockQuality.clockClass
        > E->grandMasterClockQuality.clockClass) {
      *Dbetter = PTP_FALSE;
      return 0;
    } else if (D->defaultDS.clockQuality.clockClass
        < E->grandMasterClockQuality.clockClass) {
      *Dbetter = PTP_TRUE;
      return 0;
    }
    // accuracy
    if (D->defaultDS.clockQuality.clockAccuracy
        > E->grandMasterClockQuality.clockAccuracy) {
      *Dbetter = PTP_FALSE;
      return 0;
    } else if (D->defaultDS.clockQuality.clockAccuracy
        < E->grandMasterClockQuality.clockAccuracy) {
      *Dbetter = PTP_TRUE;
      return 0;
    }
    // offsetScaledLogVariance
    if (D->defaultDS.clockQuality.offsetScaledLogVariance
        > E->grandMasterClockQuality.offsetScaledLogVariance) {
      *Dbetter = PTP_FALSE;
      return 0;
    } else if (D->defaultDS.clockQuality.offsetScaledLogVariance
        < E->grandMasterClockQuality.offsetScaledLogVariance) {
      *Dbetter = PTP_TRUE;
      return 0;
    }
    // priority2
    if (D->defaultDS.priority2 > E->grandMasterPriority2) {
      *Dbetter = PTP_FALSE;
      return 0;
    } else if (D->defaultDS.priority2 < E->grandMasterPriority2) {
      *Dbetter = PTP_TRUE;
      return 0;
    }
    // Identity
    switch (ptp_clockIdentityCompare(*(D->defaultDS.clockIdentity),
        E->grandMasterIdentity)) {
    case 1:
      *Dbetter = PTP_FALSE;
      return 0;
    case -1:
      *Dbetter = PTP_TRUE;
      return 0;
    default:
      break;
    }
  }
  return 2;
}

void ptp_storeForeignMaster(ptp_octet *pBuf, ptp_uinteger8 portNumber) {
  UInt8 portIndex;

  portIndex = portNumber - 1;
  // only one foreign master is recorded in this program, to save memory

  // If message from current master clocks
  if ((ptp_clockIdentityCompare(ptp_foreignMaster[portIndex][0].clockIdentity,
      &(pBuf[20])) == 0)
      && !(~ptp_foreignMaster[portIndex][0].foreignMasterAnnounceMessages)) {
    // Foreign master is registered
    ptp_foreignMaster[portIndex][0].foreignMasterAnnounceMessages++;
  } else {
    // Foreign master is not registered.
    // Normal behavior would be register if there is resources, ignore if there isn't.
    // On this program, each port connects with only one other device, if a packet
    // from another device was received, only that device is accessible, only one record
    // exists on the foreignMasterDS.
    ptp_rcvOctets(8, &(pBuf[20]),
        ptp_foreignMaster[portIndex][0].clockIdentity);
    ptp_foreignMaster[portIndex][0].foreignMasterAnnounceMessages = 0;
  };

  // the following data set is used to store data that will be used on BMC algorithm
  ptp_announce[portIndex].grandMasterPriority1 = ptp_rcvUInt8(&(pBuf[47]));
  ptp_rcvOctets(8, &(pBuf[53]), ptp_announce[portIndex].grandMasterIdentity);
  ptp_announce[portIndex].grandMasterClockQuality.clockClass = ptp_rcvUInt8(
      &(pBuf[48]));
  ptp_announce[portIndex].grandMasterClockQuality.clockAccuracy =
      (ptp_clockAccuracy) ptp_rcvUInt8(&(pBuf[49]));
  ptp_announce[portIndex].grandMasterClockQuality.offsetScaledLogVariance =
      ptp_rcvUInt16(&(pBuf[50]));
  ptp_announce[portIndex].grandMasterPriority2 = ptp_rcvUInt8(&(pBuf[52]));
  ptp_announce[portIndex].stepsRemoved = ptp_rcvUInt16(&(pBuf[61]));
  ptp_announce[portIndex].currentUtcOffset = ptp_rcvUInt16(&(pBuf[44]));
  ptp_announce[portIndex].currentUtcOffsetValid = (pBuf[6] >> 2) & 0x01;
  ptp_announce[portIndex].leap59 = (pBuf[6] >> 1) & 0x01;
  ptp_announce[portIndex].leap61 = pBuf[6] & 0x01;
  ptp_announce[portIndex].timeTraceable = (pBuf[6] >> 4) & 0x01;
  ptp_announce[portIndex].frequencyTraceable = (pBuf[6] >> 5) & 0x01;
  ptp_announce[portIndex].ptpTimescale = (pBuf[6] >> 3) & 0x01;
  ptp_announce[portIndex].timeSource = (ptp_timeSource) ptp_rcvUInt8(
      &(pBuf[63]));
}

ptp_uinteger8 ptp_startFrame(ptp_uinteger16 destination, ptp_uinteger8 len) {
  return nwk_startFrame(destination, NWK_CMDIDPTP, 1, len);
}

ptp_uinteger8 ptp_snd(ptp_uinteger8 len, void *pBuf) {
  return nwk_snd(len, pBuf);
}

void ptp_endFrame() {
  nwk_endFrame(TRUE);
}

ptp_uinteger8 ptp_sndUInt8(ptp_uinteger8 v) {
  return nwk_sndUInt8(v);
}

ptp_uinteger8 ptp_sndUInt16(ptp_uinteger16 v) {
  return nwk_sndUInt16(v);
}

ptp_uinteger8 ptp_sndUInt32(ptp_uinteger32 *p) {
  return nwk_sndUInt32(p);
}

ptp_uinteger8 ptp_sndUInt48(ptp_uinteger48 *p) {
  return nwk_sndUInt48(p);
}

ptp_uinteger8 ptp_sndUInt48sc(PTP_SecondsCount *p) {
  if (nwk_sndUInt32(&(p->secondsLow)))
    return nwk_sndUInt16(p->secondsHigh);
  else
    return FALSE;
}

ptp_uinteger8 ptp_sndUInt64(ptp_uinteger64 *p) {
  return nwk_sndUInt64(p);
}

ptp_uinteger8 ptp_sndInt64(ptp_integer64 *p) {
  return nwk_sndInt64(p);
}

ptp_uinteger8 ptp_rcvUInt8(ptp_octet *pBuf) {
  return pBuf[0];
}

ptp_uinteger16 ptp_rcvUInt16(ptp_octet *pBuf) {
  ptp_uinteger32 r;
  ((char *) &r)[0] = pBuf[0];
  ((char *) &r)[1] = pBuf[1];
  return r;
}

ptp_uinteger32 ptp_rcvUInt32(ptp_octet *pBuf) {
  ptp_uinteger32 r;
  ((char *) &r)[0] = pBuf[0];
  ((char *) &r)[1] = pBuf[1];
  ((char *) &r)[2] = pBuf[2];
  ((char *) &r)[3] = pBuf[3];
  return r;
}

ptp_uinteger48 ptp_rcvUInt48(ptp_octet *pBuf) {
  ptp_uinteger48 r;
  ((char *) &r)[0] = pBuf[0];
  ((char *) &r)[1] = pBuf[1];
  ((char *) &r)[2] = pBuf[2];
  ((char *) &r)[3] = pBuf[3];
  ((char *) &r)[4] = pBuf[4];
  ((char *) &r)[5] = pBuf[5];
  ((char *) &r)[6] = 0;
  ((char *) &r)[7] = 0;
  return r;
}

void ptp_rcvUInt48sc(ptp_octet *pBuf, PTP_SecondsCount *p) {
  ((char *) &(p->secondsLow))[0] = pBuf[0];
  ((char *) &(p->secondsLow))[1] = pBuf[1];
  ((char *) &(p->secondsLow))[2] = pBuf[2];
  ((char *) &(p->secondsLow))[3] = pBuf[3];
  ((char *) &(p->secondsHigh))[0] = pBuf[4];
  ((char *) &(p->secondsHigh))[1] = pBuf[5];
}

ptp_uinteger64 ptp_rcvUInt64(ptp_octet *pBuf) {
  ptp_uinteger64 r;
  ((char *) &r)[0] = pBuf[0];
  ((char *) &r)[1] = pBuf[1];
  ((char *) &r)[2] = pBuf[2];
  ((char *) &r)[3] = pBuf[3];
  ((char *) &r)[4] = pBuf[4];
  ((char *) &r)[5] = pBuf[5];
  ((char *) &r)[6] = pBuf[6];
  ((char *) &r)[7] = pBuf[7];
  return r;
}

ptp_integer64 ptp_rcvInt64(ptp_octet *pBuf) {
  ptp_integer64 r;
  ((char *) &r)[0] = pBuf[0];
  ((char *) &r)[1] = pBuf[1];
  ((char *) &r)[2] = pBuf[2];
  ((char *) &r)[3] = pBuf[3];
  ((char *) &r)[4] = pBuf[4];
  ((char *) &r)[5] = pBuf[5];
  ((char *) &r)[6] = pBuf[6];
  ((char *) &r)[7] = pBuf[7];
  return r;
}

void ptp_rcvOctets(ptp_uinteger8 len, ptp_octet *pBuf, ptp_octet *dest) {
  ptp_copyOctets(len, pBuf, dest);
}

void ptp_copyOctets(ptp_uinteger8 len, ptp_octet *source, ptp_octet *dest) {
  while (len) {
    len--;
    *dest++ = *source++;
  };
}