/* Pack Sync message into out buffer of ppi */
static void msg_pack_sync(struct pp_instance *ppi, Timestamp *orig_tstamp)
{
void *buf;
buf = ppi->tx_ptp;
/* changes in header */
*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
/* RAZ messageType */
*(char *)(buf + 0) = *(char *)(buf + 0) | 0x00;
/* Table 19 */
*(UInteger16 *) (buf + 2) = htons(PP_SYNC_LENGTH);
ppi->sent_seq[PPM_SYNC]++;
*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_SYNC]);
*(UInteger8 *) (buf + 32) = 0x00;
/* Table 23 */
*(Integer8 *) (buf + 33) = DSPOR(ppi)->logSyncInterval;
memset((buf + 8), 0, 8);
/* Sync message */
*(UInteger16 *) (buf + 34) = htons(orig_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) = htonl(orig_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) = htonl(orig_tstamp->nanosecondsField);
}
/* Pack Follow Up message into out buffer of ppi*/
static void msg_pack_follow_up(struct pp_instance *ppi, Timestamp *prec_orig_tstamp)
{
void *buf;
buf = ppi->tx_ptp;
/* changes in header */
*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
/* RAZ messageType */
*(char *)(buf + 0) = *(char *)(buf + 0) | 0x08;
/* Table 19 */
*(UInteger16 *) (buf + 2) = htons(PP_FOLLOW_UP_LENGTH);
*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_SYNC]);
/* sentSyncSequenceId has already been incremented in msg_issue_sync */
*(UInteger8 *) (buf + 32) = 0x02;
/* Table 23 */
*(Integer8 *) (buf + 33) = DSPOR(ppi)->logSyncInterval;
/* Follow Up message */
*(UInteger16 *) (buf + 34) =
htons(prec_orig_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) =
htonl(prec_orig_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) =
htonl(prec_orig_tstamp->nanosecondsField);
}
/* Pack Announce message into out buffer of ppi */
static int msg_pack_announce(struct pp_instance *ppi)
{
void *buf;
buf = ppi->tx_ptp;
/* changes in header */
*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
/* RAZ messageType */
*(char *)(buf + 0) = *(char *)(buf + 0) | 0x0B;
/* Table 19 */
*(UInteger16 *) (buf + 2) = htons(PP_ANNOUNCE_LENGTH);
ppi->sent_seq[PPM_ANNOUNCE]++;
*(UInteger16 *) (buf + 30) = htons(ppi->sent_seq[PPM_ANNOUNCE]);
*(UInteger8 *) (buf + 32) = 0x05;
/* Table 23 */
*(Integer8 *) (buf + 33) = DSPOR(ppi)->logAnnounceInterval;
/* Announce message */
memset((buf + 34), 0, 10);
*(Integer16 *) (buf + 44) = htons(DSPRO(ppi)->currentUtcOffset);
*(UInteger8 *) (buf + 47) = DSPAR(ppi)->grandmasterPriority1;
*(UInteger8 *) (buf + 48) = DSDEF(ppi)->clockQuality.clockClass;
*(Enumeration8 *) (buf + 49) = DSDEF(ppi)->clockQuality.clockAccuracy;
*(UInteger16 *) (buf + 50) =
htons(DSDEF(ppi)->clockQuality.offsetScaledLogVariance);
*(UInteger8 *) (buf + 52) = DSPAR(ppi)->grandmasterPriority2;
memcpy((buf + 53), &DSPAR(ppi)->grandmasterIdentity,
PP_CLOCK_IDENTITY_LENGTH);
*(UInteger16 *) (buf + 61) = htons(DSCUR(ppi)->stepsRemoved);
*(Enumeration8 *) (buf + 63) = DSPRO(ppi)->timeSource;
if (pp_hooks.pack_announce)
return pp_hooks.pack_announce(ppi);
return PP_ANNOUNCE_LENGTH;
}
/* Pack header message into out buffer of ppi */
void msg_pack_header(struct pp_instance *ppi, void *buf)
{
/* (spec annex D and F) */
*(UInteger8 *) (buf + 0) = 0; /* message type changed later */
*(UInteger4 *) (buf + 1) = DSPOR(ppi)->versionNumber;
*(UInteger8 *) (buf + 4) = DSDEF(ppi)->domainNumber;
*(UInteger8 *) (buf + 6) = PP_TWO_STEP_FLAG;
memset((buf + 8), 0, 8);
memcpy((buf + 20), &DSPOR(ppi)->portIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH);
*(UInteger16 *) (buf + 28) =
htons(DSPOR(ppi)->portIdentity.portNumber);
*(UInteger8 *) (buf + 33) = 0x7F;
/* Default value(spec Table 24) */
}
/* Unpack header from in buffer to msg_tmp_header field */
int msg_unpack_header(struct pp_instance *ppi, void *buf, int plen)
{
MsgHeader *hdr = &ppi->received_ptp_header;
hdr->transportSpecific = (*(Nibble *) (buf + 0)) >> 4;
hdr->messageType = (*(Enumeration4 *) (buf + 0)) & 0x0F;
hdr->versionPTP = (*(UInteger4 *) (buf + 1)) & 0x0F;
/* force reserved bit to zero if not */
hdr->messageLength = htons(*(UInteger16 *) (buf + 2));
hdr->domainNumber = (*(UInteger8 *) (buf + 4));
memcpy(hdr->flagField, (buf + 6), PP_FLAG_FIELD_LENGTH);
memcpy(&hdr->correctionfield.msb, (buf + 8), 4);
memcpy(&hdr->correctionfield.lsb, (buf + 12), 4);
hdr->correctionfield.msb = htonl(hdr->correctionfield.msb);
hdr->correctionfield.lsb = htonl(hdr->correctionfield.lsb);
memcpy(&hdr->sourcePortIdentity.clockIdentity, (buf + 20),
PP_CLOCK_IDENTITY_LENGTH);
hdr->sourcePortIdentity.portNumber =
htons(*(UInteger16 *) (buf + 28));
hdr->sequenceId = htons(*(UInteger16 *) (buf + 30));
hdr->controlField = (*(UInteger8 *) (buf + 32));
hdr->logMessageInterval = (*(Integer8 *) (buf + 33));
/*
* If the message is from us, we should discard it.
* The best way to do that is comparing the mac address,
* but it's easier to check the clock identity (we refuse
* any port, not only the same port, as we can't sync with
* ourself even when we'll run in multi-port mode.
*/
if (!memcmp(&ppi->received_ptp_header.sourcePortIdentity.clockIdentity,
&DSPOR(ppi)->portIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH))
return -1;
/*
* This FLAG_FROM_CURRENT_PARENT must be killed. Meanwhile, say it's
* from current parent if we have no current parent, so the rest works
*/
if (!DSPAR(ppi)->parentPortIdentity.portNumber ||
(!memcmp(&DSPAR(ppi)->parentPortIdentity.clockIdentity,
&hdr->sourcePortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH) &&
(DSPAR(ppi)->parentPortIdentity.portNumber ==
hdr->sourcePortIdentity.portNumber)))
ppi->flags |= PPI_FLAG_FROM_CURRENT_PARENT;
else
ppi->flags &= ~PPI_FLAG_FROM_CURRENT_PARENT;
return 0;
}
/* pack DelayResp message into OUT buffer of ppi */
static void msg_pack_delay_resp(struct pp_instance *ppi,
MsgHeader *hdr, Timestamp *rcv_tstamp)
{
void *buf;
buf = ppi->tx_ptp;
/* changes in header */
*(char *)(buf + 0) = *(char *)(buf + 0) & 0xF0;
/* RAZ messageType */
*(char *)(buf + 0) = *(char *)(buf + 0) | 0x09;
/* Table 19 */
*(UInteger16 *) (buf + 2) = htons(PP_DELAY_RESP_LENGTH);
*(UInteger8 *) (buf + 4) = hdr->domainNumber;
memset((buf + 8), 0, 8);
/* Copy correctionField of delayReqMessage */
*(Integer32 *) (buf + 8) = htonl(hdr->correctionfield.msb);
*(Integer32 *) (buf + 12) = htonl(hdr->correctionfield.lsb);
*(UInteger16 *) (buf + 30) = htons(hdr->sequenceId);
*(UInteger8 *) (buf + 32) = 0x03;
/* Table 23 */
*(Integer8 *) (buf + 33) = DSPOR(ppi)->logMinDelayReqInterval;
/* Table 24 */
/* Delay_resp message */
*(UInteger16 *) (buf + 34) =
htons(rcv_tstamp->secondsField.msb);
*(UInteger32 *) (buf + 36) = htonl(rcv_tstamp->secondsField.lsb);
*(UInteger32 *) (buf + 40) = htonl(rcv_tstamp->nanosecondsField);
memcpy((buf + 44), &hdr->sourcePortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH);
*(UInteger16 *) (buf + 52) =
htons(hdr->sourcePortIdentity.portNumber);
}
int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
int e = 0; /* error var, to check errors in msg handling */
MsgHeader *hdr = &ppi->received_ptp_header;
MsgDelayResp resp;
int d1, d2;
if (ppi->is_new_state) {
pp_servo_init(ppi);
if (pp_hooks.new_slave)
e = pp_hooks.new_slave(ppi, pkt, plen);
if (e)
goto out;
ppi->waiting_for_follow = FALSE;
pp_timeout_restart_annrec(ppi);
pp_timeout_rand(ppi, PP_TO_DELAYREQ,
DSPOR(ppi)->logMinDelayReqInterval);
}
if (plen == 0)
goto out;
switch (hdr->messageType) {
case PPM_ANNOUNCE:
e = st_com_slave_handle_announce(ppi, pkt, plen);
break;
case PPM_SYNC:
e = st_com_slave_handle_sync(ppi, pkt, plen);
break;
case PPM_FOLLOW_UP:
e = st_com_slave_handle_followup(ppi, pkt, plen);
break;
case PPM_DELAY_REQ:
/* Being slave, we are not waiting for a delay request */
break;
case PPM_DELAY_RESP:
e = (plen < PP_DELAY_RESP_LENGTH);
if (e)
break;
msg_unpack_delay_resp(pkt, &resp);
if ((memcmp(&DSPOR(ppi)->portIdentity.clockIdentity,
&resp.requestingPortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH) == 0) &&
((ppi->sent_seq[PPM_DELAY_REQ]) ==
hdr->sequenceId) &&
(DSPOR(ppi)->portIdentity.portNumber ==
resp.requestingPortIdentity.portNumber)
&& ppi->is_from_cur_par) {
to_TimeInternal(&ppi->t4, &resp.receiveTimestamp);
/*
* FIXME: how is correctionField handled in t3/t4?
* I think the master should consider it when
* generating t4, and report back a modified t4
*/
if (pp_hooks.handle_resp)
e = pp_hooks.handle_resp(ppi);
else
pp_servo_got_resp(ppi);
if (e)
goto out;
ppi->log_min_delay_req_interval =
hdr->logMessageInterval;
} else {
pp_diag(ppi, frames, 2, "pp_slave : "
"Delay Resp doesn't match Delay Req\n");
}
break;
/*
* We are not supporting pdelay (not configured to, see
* 9.5.13.1, p 106), so all the code about pdelay is removed
* as a whole by one commit in our history. It can be recoverd
* and fixed if needed
*/
default:
/* disregard, nothing to do */
break;
}
out:
if (e == 0)
e = st_com_execute_slave(ppi);
if (pp_timeout_z(ppi, PP_TO_DELAYREQ)) {
e = msg_issue_delay_req(ppi);
ppi->t3 = ppi->last_snt_time;
/* Restart the timeout for next time */
pp_timeout_rand(ppi, PP_TO_DELAYREQ,
DSPOR(ppi)->logMinDelayReqInterval);
/* Add latency */
add_TimeInternal(&ppi->t3,
&ppi->t3,
&OPTS(ppi)->outbound_latency);
}
if (e) {
ppi->next_state = PPS_FAULTY;
return 0;
}
/* Leaving this state */
if (ppi->next_state != ppi->state) {
pp_timeout_clr(ppi, PP_TO_ANN_RECEIPT);
pp_timeout_clr(ppi, PP_TO_DELAYREQ);
pp_servo_init(ppi);
}
d1 = d2 = pp_ms_to_timeout(ppi, PP_TO_ANN_RECEIPT);
if (ppi->timeouts[PP_TO_DELAYREQ])
d2 = pp_ms_to_timeout(ppi, PP_TO_DELAYREQ);
ppi->next_delay = d1 < d2 ? d1 : d2;
return 0;
}
int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
int msgtype, d1, d2;
int e = 0; /* error var, to check errors in msg handling */
if (ppi->is_new_state) {
pp_timeout_rand(ppi, PP_TO_SYNC, DSPOR(ppi)->logSyncInterval);
pp_timeout_rand(ppi, PP_TO_ANN_INTERVAL,
DSPOR(ppi)->logAnnounceInterval);
/* Send an announce immediately, when becomes master */
if ((e = msg_issue_announce(ppi)) < 0)
goto out;
}
if (pp_timeout_z(ppi, PP_TO_SYNC)) {
if ((e = msg_issue_sync_followup(ppi) < 0))
goto out;
/* Restart the timeout for next time */
pp_timeout_rand(ppi, PP_TO_SYNC, DSPOR(ppi)->logSyncInterval);
}
if (pp_timeout_z(ppi, PP_TO_ANN_INTERVAL)) {
if ((e = msg_issue_announce(ppi) < 0))
goto out;
/* Restart the timeout for next time */
pp_timeout_rand(ppi, PP_TO_ANN_INTERVAL,
DSPOR(ppi)->logAnnounceInterval);
}
if (plen == 0)
goto out;
/*
* An extension can do special treatment of this message type,
* possibly returning error or eating the message by returning
* PPM_NOTHING_TO_DO
*/
msgtype = ppi->received_ptp_header.messageType;
if (pp_hooks.master_msg)
msgtype = pp_hooks.master_msg(ppi, pkt, plen, msgtype);
if (msgtype < 0) {
e = msgtype;
goto out;
}
switch (msgtype) {
case PPM_NOTHING_TO_DO:
break;
case PPM_ANNOUNCE:
e = st_com_master_handle_announce(ppi, pkt, plen);
break;
case PPM_SYNC:
e = st_com_master_handle_sync(ppi, pkt, plen);
break;
case PPM_DELAY_REQ:
msg_issue_delay_resp(ppi, &ppi->last_rcv_time);
break;
/*
* We are not supporting pdelay (not configured to, see
* 9.5.13.1, p 106), so all the code about pdelay is removed
* as a whole by one commit in our history. It can be recoverd
* and fixed if needed
*/
default:
/* disregard, nothing to do */
break;
}
out:
if (e == 0) {
if (DSDEF(ppi)->clockQuality.clockClass == PP_CLASS_SLAVE_ONLY
|| (ppi->role == PPSI_ROLE_SLAVE))
ppi->next_state = PPS_LISTENING;
} else {
ppi->next_state = PPS_FAULTY;
}
d1 = pp_ms_to_timeout(ppi, PP_TO_ANN_INTERVAL);
d2 = pp_ms_to_timeout(ppi, PP_TO_SYNC);
ppi->next_delay = d1 < d2 ? d1 : d2;
return 0;
}
int pp_initializing(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
unsigned char *id, *mac;
struct DSPort *port = DSPOR(ppi);
struct pp_runtime_opts *opt = OPTS(ppi);
int ret = 0;
if (ppi->n_ops->init(ppi) < 0) /* it must handle being called twice */
goto failure;
/* Clock identity comes from mac address with 0xff:0xfe intermixed */
id = (unsigned char *)&DSDEF(ppi)->clockIdentity;
mac = NP(ppi)->ch[PP_NP_GEN].addr;
id[0] = mac[0];
id[1] = mac[1];
id[2] = mac[2];
id[3] = 0xff;
id[4] = 0xfe;
id[5] = mac[3];
id[6] = mac[4];
id[7] = mac[5];
/*
* Initialize port data set
*/
memcpy(&port->portIdentity.clockIdentity,
&DSDEF(ppi)->clockIdentity, PP_CLOCK_IDENTITY_LENGTH);
port->portIdentity.portNumber = 1;
port->logMinDelayReqInterval = PP_DEFAULT_DELAYREQ_INTERVAL;
port->logAnnounceInterval = opt->announce_intvl;
port->announceReceiptTimeout = PP_DEFAULT_ANNOUNCE_RECEIPT_TIMEOUT;
port->logSyncInterval = opt->sync_intvl;
port->versionNumber = PP_VERSION_PTP;
if (pp_hooks.init)
ret = pp_hooks.init(ppi, pkt, plen);
if (ret) {
pp_diag(ppi, ext, 1, "%s: can't init extension\n", __func__);
goto failure;
}
if (ret) {
pp_diag(ppi, time, 1, "%s: can't init timers\n", __func__);
goto failure;
}
pp_init_clock(ppi);
pp_diag(ppi, bmc, 1, "clock class = %d\n",
DSDEF(ppi)->clockQuality.clockClass);
pp_diag(ppi, bmc, 1, "clock accuracy = %d\n",
DSDEF(ppi)->clockQuality.clockAccuracy);
m1(ppi);
msg_pack_header(ppi, ppi->tx_ptp); /* This is used for all tx */
if (!ppi->master_only)
ppi->next_state = PPS_LISTENING;
else
ppi->next_state = PPS_MASTER;
return 0;
failure:
ppi->next_delay = 1000; /* wait 1s before retrying */
return 0;
}
