/* ppi->port_idx port is synchronized to Ebest Table 16 (9.3.5) of the spec. */
static void s1(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann)
{
struct DSParent *parent = DSPAR(ppi);
struct DSTimeProperties *prop = DSPRO(ppi);
/* Current DS */
DSCUR(ppi)->stepsRemoved = ann->stepsRemoved + 1;
/* Parent DS */
parent->parentPortIdentity = hdr->sourcePortIdentity;
parent->grandmasterIdentity = ann->grandmasterIdentity;
parent->grandmasterClockQuality = ann->grandmasterClockQuality;
parent->grandmasterPriority1 = ann->grandmasterPriority1;
parent->grandmasterPriority2 = ann->grandmasterPriority2;
/* Timeproperties DS */
prop->timeSource = ann->timeSource;
if (prop->currentUtcOffset != ann->currentUtcOffset) {
pp_diag(ppi, bmc, 1, "New UTC offset: %i\n",
ann->currentUtcOffset);
prop->currentUtcOffset = ann->currentUtcOffset;
ppi->t_ops->set(ppi, NULL);
}
/* FIXME: can't we just copy the bit keeping values? */
prop->currentUtcOffsetValid = ((hdr->flagField[1] & FFB_UTCV) != 0);
prop->leap59 = ((hdr->flagField[1] & FFB_LI59) != 0);
prop->leap61 = ((hdr->flagField[1] & FFB_LI61) != 0);
prop->timeTraceable = ((hdr->flagField[1] & FFB_TTRA) != 0);
prop->frequencyTraceable = ((hdr->flagField[1] & FFB_FTRA) != 0);
prop->ptpTimescale = ((hdr->flagField[1] & FFB_PTP) != 0);
if (pp_hooks.s1)
pp_hooks.s1(ppi, hdr, ann);
}
int bmc(struct pp_instance *ppi)
{
struct pp_frgn_master *frgn_master = ppi->frgn_master;
int i, best;
if (!ppi->frgn_rec_num)
if (ppi->state == PPS_MASTER) {
m1(ppi);
return ppi->state;
}
/* Find Erbest, 9.3.2.3 */
for (i = 1, best = 0; i < ppi->frgn_rec_num; i++)
if (bmc_dataset_cmp(ppi, &frgn_master[i], &frgn_master[best])
< 0)
best = i;
pp_diag(ppi, bmc, 1,"Best foreign master is %i/%i\n", best,
ppi->frgn_rec_num);
if (ppi->frgn_rec_best != best) {
ppi->frgn_rec_best = best;
bmc_update_ebest(GLBS(ppi));
}
return bmc_state_decision(ppi, &frgn_master[best]);
}
int wrs_read_calibration_data(struct pp_instance *ppi,
uint32_t *delta_tx, uint32_t *delta_rx, int32_t *fix_alpha,
int32_t *clock_period)
{
struct hal_port_state *p;
/* The following fields come from struct hexp_port_state */
uint32_t port_delta_tx, port_delta_rx;
int32_t port_fix_alpha;
p = pp_wrs_lookup_port(ppi->iface_name);
if (!p)
return WR_HW_CALIB_NOT_FOUND;
if(!p->calib.tx_calibrated || !p->calib.rx_calibrated)
return WR_HW_CALIB_NOT_FOUND;
/*
* Like in wrs_net_init, we build fields that were in
* hexp_port_state from the "real" hal_port_state in the same
* way as the HAL itself was doing to fill the RPC structure.
* Formulas copied from libwr/hal_shmem.c (get_exported_state).
*/
port_delta_tx = p->calib.delta_tx_phy
+ p->calib.sfp.delta_tx_ps + p->calib.delta_tx_board;
port_delta_rx = p->calib.delta_rx_phy
+ p->calib.sfp.delta_rx_ps + p->calib.delta_rx_board;
port_fix_alpha = (double)pow(2.0, 40.0) *
((p->calib.sfp.alpha + 1.0) / (p->calib.sfp.alpha + 2.0)
- 0.5);
pp_diag(ppi, servo, 1, "deltas: tx=%d, rx=%d\n",
port_delta_tx, port_delta_rx);
if(delta_tx)
*delta_tx = port_delta_tx;
if(delta_rx)
*delta_rx = port_delta_rx;
if(fix_alpha)
*fix_alpha = port_fix_alpha;
if(clock_period)
*clock_period = 16000; /* REF_CLOCK_PERIOD_PS */
return WR_HW_CALIB_OK;
}
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;
}
/* 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;
}
/*
* Data set comparison between two foreign masters. Return similar to
* memcmp(). However, lower values take precedence, so in A-B (like
* in comparisons, > 0 means B wins (and < 0 means A wins).
*/
static int bmc_dataset_cmp(struct pp_instance *ppi,
struct pp_frgn_master *a,
struct pp_frgn_master *b)
{
struct ClockQuality *qa, *qb;
struct MsgAnnounce *aa = &a->ann;
struct MsgAnnounce *ab = &b->ann;
struct ClockIdentity *ida = &a->hdr.sourcePortIdentity.clockIdentity;
struct ClockIdentity *idb = &b->hdr.sourcePortIdentity.clockIdentity;
struct ClockIdentity *idparent;
int diff;
/* dataset_cmp is called several times, so report only at level 2 */
pp_diag(ppi, bmc, 2,"%s\n", __func__);
if (!idcmp(&aa->grandmasterIdentity, &ab->grandmasterIdentity)) {
/* The grandmaster is the same: part 2, fig 28, page 90. */
diff = aa->stepsRemoved - ab->stepsRemoved;
if (diff > 1 || diff < -1)
return diff;
idparent = &DSPAR(ppi)->parentPortIdentity.clockIdentity;
if (diff > 0) {
if (!idcmp(ida, idparent)) {
pp_diag(ppi, bmc, 1,"%s:%i: Error 1\n",
__func__, __LINE__);
return 0;
}
return 1;
}
if (diff < 0) {
if (!idcmp(idb, idparent)) {
pp_diag(ppi, bmc, 1,"%s:%i: Error 1\n",
__func__, __LINE__);
return 0;
}
return -1;
}
/* stepsRemoved is equal, compare identities */
diff = idcmp(ida, idb);
if (!diff) {
pp_diag(ppi, bmc, 1,"%s:%i: Error 2\n", __func__, __LINE__);
return 0;
}
return diff;
}
/* The grandmasters are different: part 1, fig 27, page 89. */
qa = &aa->grandmasterClockQuality;
qb = &ab->grandmasterClockQuality;
if (aa->grandmasterPriority1 != ab->grandmasterPriority1)
return aa->grandmasterPriority1 - ab->grandmasterPriority1;
if (qa->clockClass != qb->clockClass)
return qa->clockClass - qb->clockClass;
if (qa->clockAccuracy != qb->clockAccuracy)
return qa->clockAccuracy - qb->clockAccuracy;
if (qa->offsetScaledLogVariance != qb->offsetScaledLogVariance)
return qa->clockClass - qb->clockClass;
if (aa->grandmasterPriority2 != ab->grandmasterPriority2)
return aa->grandmasterPriority2 - ab->grandmasterPriority2;
return idcmp(&aa->grandmasterIdentity, &ab->grandmasterIdentity);
}
/*
* Log means messages
*/
void pp_timeout_log(struct pp_instance *ppi, int index)
{
pp_diag(ppi, time, 1, "timeout expired: %s\n", timeout_names[index]);
}
int pp_faulty(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
pp_diag(ppi, fsm, 1, "Faulty state detected\n");
ppi->next_state = PPS_INITIALIZING;
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;
}
