ptp_messages.c

Go to the documentation of this file.

/*
 * ptp_messages.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
 *
 */

/*
 * ======== Standard MSP430 includes ========
 */
#include <msp430.h>

/*
 * ======== Includes ========
 */
#include "ptp_messages.h"
#include "ptp_util.h"
#include "ptp_cfg.h"
#include "ptp.h"
#include "ptp_const.h"
#include "clock.h"

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

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

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

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

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

PTP_TimeStamp ptp_P2Pt1;
ptp_uinteger16 ptp_sequencePDelayReqId[PTP_NUMBERPORTS];
PTP_P2PPDelayReqDS ptp_P2PPDelayReqQueue[PTP_P2PPDELAYREQQUEUESIZE];
ptp_uinteger8 ptp_P2PDelayReqQueueWrite;
ptp_uinteger8 ptp_P2PDelayReqQueueRead;
ptp_boolean ptp_MustAdjustAtPDelayReq;

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

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

void ptp_MsgHeaderSnd(ptp_uinteger16 sourcePortNumber, ptp_msgType msgType,
    ptp_uinteger16 messageLength, ptp_uinteger8 domainNumber,
    ptp_boolean unicast, ptp_integer64 correctionField,
    ptp_uinteger16 sequenceId) {
  ptp_uinteger16 uv16;
  ptp_integer8 v8;
  ptp_uinteger8 portIndex;

  if ((sourcePortNumber == 0xFFFF) || (!sourcePortNumber))
    portIndex = 0;
  else
    portIndex = sourcePortNumber - 1;

  // Comments are Header Field (number of Octets) session on the standard.

  ptp_sndUInt8(PTP_MSG_FIELDTRANSPORTSPECIFICVALUE | msgType); // transportSpecific + messageType (.5 + .5 = 1) 13.3.2.1 + 13.3.2.2
  ptp_sndUInt8(ptpClock.portDS[portIndex].versionNumber); // reserved + versionPTP (.5 + .5 = 1) 13.3.2.3
  ptp_sndUInt16(messageLength); // messageLength (2) 13.3.2.4
  // To add support to P2P transparent clocks, domainNumber must have the value defined in 11.4.3
  ptp_sndUInt8(domainNumber); // domainNumber (1) 13.3.2.5
  ptp_sndUInt8(0x00); // reserved (1)

  // flagField (2) 13.3.2.6 :
  // 0: No alternative masters
  uv16 = 0;
  if ((msgType == PTP_MSGTYPE_SYNC) || (msgType == PTP_MSGTYPE_PDELAY_RESP))
    uv16 |= ptpClock.defaultDS.twoStepFlag << 1;
  uv16 |= (unicast != 0) << 2;
  if (msgType == PTP_MSGTYPE_ANNOUNCE)
    uv16 |= (ptpClock.timePropertiesDS.leap61 << 8)
        | (ptpClock.timePropertiesDS.leap61 << 9)
        | (ptpClock.timePropertiesDS.currentUtcOffsetValid << 10)
        | (ptpClock.timePropertiesDS.ptpTimescale << 11)
        | (ptpClock.timePropertiesDS.timeTraceable << 12)
        | (ptpClock.timePropertiesDS.frequencyTraceable << 13);
  ptp_sndUInt16(uv16);
  // end - flagField (2) 13.3.2.6

  ptp_sndInt64(&correctionField); // correctionField (8) 13.3.2.7
  ptp_snd(4, PTP_ZEROSEQ64); // reserved (4)
  ptp_snd(8, *ptpClock.defaultDS.clockIdentity); // sourcePortIdentity (10) 13.3.2.8
  ptp_sndUInt16(sourcePortNumber);
  ptp_sndUInt16(sequenceId);  // sequenceId (2) 13.3.2.9

  // controlField (1) 13.3.2.10:
  switch (msgType) {
  case PTP_MSGTYPE_SYNC:
    v8 = 0;
    break;
  case PTP_MSGTYPE_DELAY_REQ:
    v8 = 1;
    break;
  case PTP_MSGTYPE_FOLLOW_UP:
    v8 = 2;
    break;
  case PTP_MSGTYPE_DELAY_RESP:
    v8 = 3;
    break;
  case PTP_MSGTYPE_MANAGEMENT:
    v8 = 4;
    break;
  default:
    v8 = 5;
  }
  ptp_sndUInt8(v8);
  // end - controlField (1) 13.3.2.10

  // logMessageInterval (1) 13.3.2.11:
  switch (msgType) {
  case PTP_MSGTYPE_ANNOUNCE:
    v8 = ptpClock.portDS[portIndex].logAnnounceInterval;
    break;
  case PTP_MSGTYPE_SYNC:
  case PTP_MSGTYPE_FOLLOW_UP:
    if (unicast)
      v8 = 0x7F;
    else
      v8 = ptpClock.portDS[portIndex].logAnnounceInterval;
    break;
  case PTP_MSGTYPE_DELAY_RESP:
    if (unicast)
      v8 = 0x7F;
    else
      v8 = ptpClock.portDS[portIndex].logMinDelayReqInterval;
    break;
  default:
    v8 = 0x7F;
  }
  ptp_sndUInt8(v8);
  // end - logMessageInterval (1) 13.3.2.11
}

// Not useful right now, but left for possible future use
// *
// * @brief   .
// *
// * @param[in] pBuf  Pointer to the data to parse
// */
//void ptp_MsgHeaderRcv(void *pBuf) {
//}

// Not useful right now, but left for possible future use
// * @brief   Zero suffix
// *
// * This function adds a sequence of zeros at the end of the message.
// *
// * @param[in] size  Size of suffix to add
// */
//void ptp_MsgSuffixZeroSnd(ptp_uinteger8 size) {
//}

// Not useful right now, but left for possible future use
// *
// * @brief   .
// *
// * @param[in] pBuf  Pointer to the data to parse
// */
//void ptp_MsgSuffixZeroRcv(void *pBuf) {
//}

void ptp_MsgSyncSnd() {
}

void ptp_MsgSyncRcv(ptp_octet *pBuf) {
}

void ptp_MsgDelayReqSnd() {
}

void ptp_MsgDelayReqRcv(ptp_octet *pBuf) {
}

ptp_uinteger8 ptp_MsgPdelayReqSnd(ptp_uinteger8 portNumber) {

  if (ptp_startFrame(nwk_getNeighborAddress(portNumber), 54)) {
    portNumber--; // portIndex from here
    ptp_sequencePDelayReqId[portNumber]++;
    // header
    ptp_getCurrentTime(&(ptp_P2Pt1));
    ptp_MsgHeaderSnd(portNumber + 1, PTP_MSGTYPE_PDELAY_REQ, 54, 0,
        PTP_FALSE, 0, ptp_sequencePDelayReqId[portNumber]);
    ptp_sndUInt48sc(&(ptp_P2Pt1.secondsField));
    ptp_sndUInt32(&(ptp_P2Pt1.nanoSecondsField));
    ptp_snd(8, PTP_ZEROSEQ64); // 10 bytes reserved
    ptp_snd(2, PTP_ZEROSEQ64);
    ptp_endFrame();
    return 1;
  }
  return 0;
}

void ptp_MsgPdelayReqRcv(ptp_octet *pBuf, PTP_TimeStamp *t2,
    ptp_uinteger8 portNumber) {
  PTP_TimeStamp t1;

  // If message is from master and there is a registered ptp_meanPathDelay, synchronize the time
  portNumber--; // portIndex from here
  if (ptp_MustAdjustAtPDelayReq && ptp_meanPathDelay[portNumber]
      && (((ptp_uinteger32) (ptp_meanPathDelay[portNumber] >> 16))
          < 1000000000u)
      && (ptpClock.parentDS.parentPortIdentity.clockIdentity
          == &(ptp_foreignMaster[portNumber][0].clockIdentity))) {
    ptp_MustAdjustAtPDelayReq = PTP_FALSE;
    ptp_rcvUInt48sc(&(pBuf[34]), &(t1.secondsField));
    t1.nanoSecondsField = ptp_rcvUInt32(&(pBuf[40]));

    ptp_adjTime(&t1, t2, portNumber);
  }

  // if there isn't space on the queue, ignore
  if ((ptp_P2PDelayReqQueueWrite + 1 == ptp_P2PDelayReqQueueRead)
      || ((ptp_P2PDelayReqQueueWrite + 1 == PTP_P2PPDELAYREQQUEUESIZE)
          && (ptp_P2PDelayReqQueueRead == 0)))
    return;

  ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueWrite].localPortNumber =
      portNumber + 1;
  ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueWrite].t2.secondsField.secondsLow =
      t2->secondsField.secondsLow;
  ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueWrite].t2.secondsField.secondsHigh =
      t2->secondsField.secondsHigh;
  ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueWrite].t2.nanoSecondsField =
      t2->nanoSecondsField;
  ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueWrite].sequenceId = ptp_rcvUInt16(
      &(pBuf[30]));
  ptp_rcvOctets(8, &(pBuf[20]),
      ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueWrite].clockIdentity);
  ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueWrite].domain = ptp_rcvUInt16(
      &(pBuf[4]));
  ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueWrite].correctionField =
      ptp_rcvInt64(&(pBuf[8]));

  ptp_P2PDelayReqQueueWrite++;
  if (ptp_P2PDelayReqQueueWrite == PTP_P2PPDELAYREQQUEUESIZE) {
    ptp_P2PDelayReqQueueWrite = 0;
  }
}

void ptp_MsgPdelayReqCheck() {
  if (ptp_P2PDelayReqQueueRead != ptp_P2PDelayReqQueueWrite) {
    ptp_MsgPdelayRespSnd();
  }
}

ptp_uinteger8 ptp_MsgPdelayRespSnd() {
  PTP_TimeStamp t3;

  if (ptp_startFrame(
      nwk_getNeighborAddress(
          ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueRead].localPortNumber),
      54)) {
    ptp_getCurrentTime(&t3);
    ptp_MsgHeaderSnd(
        ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueRead].localPortNumber,
        PTP_MSGTYPE_PDELAY_RESP, 54,
        ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueRead].domain,
        PTP_FALSE,
        ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueRead].correctionField
            + ptp_timeDiff(
                &(ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueRead].t2),
                &t3),
        ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueRead].sequenceId);
    ptp_sndUInt48sc(&(t3.secondsField));
    ptp_sndUInt32(&(t3.nanoSecondsField));
    ptp_snd(8,
        &(ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueRead].clockIdentity));
    ptp_sndUInt16(
        nwk_getNeighborForeignPort(
            ptp_P2PPDelayReqQueue[ptp_P2PDelayReqQueueRead].localPortNumber));
    ptp_endFrame();

    ptp_P2PDelayReqQueueRead++;
    if (ptp_P2PDelayReqQueueRead == PTP_P2PPDELAYREQQUEUESIZE) {
      ptp_P2PDelayReqQueueRead = 0;
    }
    return 1;
  }
  return 0;
}

void ptp_MsgPdelayRespRcv(ptp_octet *pBuf, PTP_TimeStamp *t4,
    ptp_uinteger8 portNumber) {
  PTP_TimeStamp t3;

  // test for 2 step flag (13.3.1, 13.3.2.6)
  if (ptp_rcvUInt16(&(pBuf[6])) & 0x0002)
    ; // no support for 2 step clock
  else {
    portNumber--; // portIndex from here
    // if doesn't match last sequenceId, ignore
    if (ptp_rcvUInt16(&(pBuf[30])) != ptp_sequencePDelayReqId[portNumber])
      return;
    ptp_meanPathDelay[portNumber] = ptp_timeDiff(&ptp_P2Pt1, t4);
    ptp_meanPathDelay[portNumber] -= (ptp_rcvInt64(&(pBuf[8])));
    ptp_meanPathDelay[portNumber] >>= 1;

    // If received message have a timestamp, use it to adjust the clock,
    // else request the adjust to be done at next PdelayReq
    ptp_rcvUInt48sc(&(pBuf[34]), &(t3.secondsField));
    t3.nanoSecondsField = ptp_rcvUInt32(&(pBuf[40]));
    if ((t3.secondsField.secondsLow == 0) && (t3.nanoSecondsField == 0)) {
      // there no timestamp, adjust the clock at PdelayReq
      ptp_MustAdjustAtPDelayReq = PTP_TRUE;
    } else {
      // there is a timestamp, adjust the clock here
      if ((((ptp_uinteger32) (ptp_meanPathDelay[portNumber] >> 16))
          < 1000000000u)
          && (ptpClock.parentDS.parentPortIdentity.clockIdentity
              == &(ptp_foreignMaster[portNumber][0].clockIdentity))) {
        ptp_adjTime(&t3, t4, portNumber);
      }
    }
  }
}

ptp_uinteger8 ptp_MsgAnnounceSnd() {
  static ptp_uinteger16 sequenceId = 0;
  PTP_TimeStamp ts;

  if (ptp_startFrame(0xFFFF, 64)) {
    // header
    // The portNumber 0xFFFF indicates "all ports" for management and signaling messages.
    // This is not in the standard.
    ptp_MsgHeaderSnd(0xFFFF, PTP_MSGTYPE_ANNOUNCE, 64, 0, PTP_FALSE, 0,
        sequenceId);

    // originTimeStamp 10:
    ptp_getCurrentTime(&ts);
    ptp_sndUInt48sc(&(ts.secondsField));
    ptp_sndUInt32(&(ts.nanoSecondsField));
    // originTimeStamp 10

    ptp_sndUInt16(ptpClock.timePropertiesDS.currentUtcOffset); // currentUtcOffset 2
    ptp_sndUInt8(0); // reserved 1
    ptp_sndUInt8(ptpClock.parentDS.grandMasterPriority1); // grandmasterPriority1 1

    // grandmasterClockQuality 4:
    ptp_sndUInt8(ptpClock.parentDS.grandMasterclockQuality.clockClass);
    ptp_sndUInt8(ptpClock.parentDS.grandMasterclockQuality.clockAccuracy);
    ptp_sndUInt16(
        ptpClock.parentDS.grandMasterclockQuality.offsetScaledLogVariance);
    // grandmasterClockQuality 4

    ptp_sndUInt8(ptpClock.parentDS.grandMasterPriority2); // grandmasterPriority2 1
    ptp_snd(8, ptpClock.parentDS.grandMasterIdentity); // grandmasterIdentity 8
    ptp_sndUInt16(ptpClock.currentDS.stepsRemoved); // stepsRemoved 2
    ptp_sndUInt8(ptpClock.timePropertiesDS.timeSource); // timeSource 1

    // Message is send in broadcast
    ptp_endFrame();
    sequenceId++;
    return 1;
  }
  return 0;
}

void ptp_MsgAnnounceRcv(ptp_octet *pBuf, ptp_uinteger8 portNumber) {

  //Check state of the port
  if ((ptpClock.portDS[0].portState == PTP_PS_INITIALIZING)
      || (ptpClock.portDS[0].portState == PTP_PS_DISABLED))
    return;
  if (ptpClock.portDS[0].portState == PTP_PS_FAULTY)
    return;

  // store foreign master or update if its known
  ptp_storeForeignMaster(pBuf, portNumber);

}

void ptp_MsgFollowUpSnd() {
}

void ptp_MsgFollowUpRcv(ptp_octet *pBuf) {
}

void ptp_MsgDelayRespSnd() {
}

void ptp_MsgDelayRespRcv(ptp_octet *pBuf) {
}

void ptp_MsgPdelayRespFollowUpSnd() {
}

void ptp_MsgPdelayRespFollowUpRcv(ptp_octet *pBuf) {
}

void ptp_MsgManagementSnd() {
}

void ptp_MsgManagementRcv(ptp_octet *pBuf) {
}

void ptp_MsgSignalingSnd() {
}

void ptp_MsgSignalingRcv(ptp_octet *pBuf) {
}