/* 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;
}
void pp_timeout_rand(struct pp_instance *ppi, int index, int logval)
{
static uint32_t seed;
uint32_t rval;
int millisec;
if (!seed) {
uint32_t *p;
/* use the least 32 bits of the mac address as seed */
p = (void *)(&DSDEF(ppi)->clockIdentity)
+ sizeof(ClockIdentity) - 4;
seed = *p;
}
/* From uclibc: they make 11 + 10 + 10 bits, we stop at 21 */
seed *= 1103515245;
seed += 12345;
rval = (unsigned int) (seed / 65536) % 2048;
seed *= 1103515245;
seed += 12345;
rval <<= 10;
rval ^= (unsigned int) (seed / 65536) % 1024;
millisec = (1 << logval) * 400; /* This is 40% of the nominal value */
millisec = (millisec * 2) + rval % millisec;
pp_timeout_set(ppi, index, millisec);
}
/* Copy local data set into header and ann message. 9.3.4 table 12. */
static void copy_d0(struct pp_instance *ppi, struct pp_frgn_master *m)
{
struct DSDefault *defds = DSDEF(ppi);
struct MsgHeader *hdr = &m->hdr;
struct MsgAnnounce *ann = &m->ann;
ann->grandmasterIdentity = defds->clockIdentity;
ann->grandmasterClockQuality = defds->clockQuality;
ann->grandmasterPriority1 = defds->priority1;
ann->grandmasterPriority2 = defds->priority2;
ann->stepsRemoved = 0;
hdr->sourcePortIdentity.clockIdentity = defds->clockIdentity;
}
/* 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) */
}
/* ppi->port_idx port is becoming Master. Table 13 (9.3.5) of the spec. */
void m1(struct pp_instance *ppi)
{
struct DSParent *parent = DSPAR(ppi);
struct DSDefault *defds = DSDEF(ppi);
/* Current data set update */
DSCUR(ppi)->stepsRemoved = 0;
clear_TimeInternal(&DSCUR(ppi)->offsetFromMaster);
clear_TimeInternal(&DSCUR(ppi)->meanPathDelay);
/* Parent data set: we are the parent */
memset(parent, 0, sizeof(*parent));
parent->parentPortIdentity.clockIdentity = defds->clockIdentity;
/* FIXME: the port? */
/* Copy grandmaster params from our defds (FIXME: is ir right?) */
parent->grandmasterIdentity = defds->clockIdentity;
parent->grandmasterClockQuality = defds->clockQuality;
parent->grandmasterPriority1 = defds->priority1;
parent->grandmasterPriority2 = defds->priority2;
/* Time Properties data set */
DSPRO(ppi)->timeSource = INTERNAL_OSCILLATOR;
}
/* State decision algorithm 9.3.3 Fig 26 */
static int bmc_state_decision(struct pp_instance *ppi,
struct pp_frgn_master *m)
{
int cmpres;
struct pp_frgn_master myself;
if (ppi->master_only)
goto master;
if (ppi->slave_only)
goto slave;
if ((!ppi->frgn_rec_num) && (ppi->state == PPS_LISTENING))
return PPS_LISTENING;
/* copy local information to a foreign_master structure */
copy_d0(ppi, &myself);
/* dataset_cmp is "a - b" but lower values win */
cmpres = bmc_dataset_cmp(ppi, &myself, m);
if (DSDEF(ppi)->clockQuality.clockClass < 128) {
if (cmpres < 0)
goto master;
if (cmpres > 0)
goto passive;
}
if (cmpres < 0)
goto master;
if (cmpres > 0) {
if (DSDEF(ppi)->numberPorts == 1)
goto slave; /* directly skip to ordinary clock handling */
else
goto check_boundary_clk;
}
pp_diag(ppi, bmc, 1,"%s: error\n", __func__);
/* MB: Is this the return code below correct? */
/* Anyway, it's a valid return code. */
return PPS_FAULTY;
check_boundary_clk:
if (ppi->port_idx == GLBS(ppi)->ebest_idx) /* This port is the Ebest */
goto slave;
/* If idcmp returns 0, it means that this port is not the best because
* Ebest is better by topology than Erbest */
if (!idcmp(&myself.ann.grandmasterIdentity,
&m->ann.grandmasterIdentity))
goto passive;
else
goto master;
passive:
p1(ppi, &m->hdr, &m->ann);
pp_diag(ppi, bmc, 1,"%s: passive\n", __func__);
return PPS_PASSIVE;
master:
m1(ppi);
pp_diag(ppi, bmc, 1,"%s: master\n", __func__);
return PPS_MASTER;
slave:
s1(ppi, &m->hdr, &m->ann);
pp_diag(ppi, bmc, 1,"%s: slave\n", __func__);
return PPS_SLAVE;
}
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;
}