Implement the port peer delay mechanism.
Signed-off-by: Richard Cochran <richardcochran@gmail.com>master
Richard Cochran
parent
806d1e9b77
commit
4df18c6fc9
271
port.c
271
port.c
|
|
@ -25,6 +25,7 @@
|
|||
|
||||
#include "bmc.h"
|
||||
#include "clock.h"
|
||||
#include "mave.h"
|
||||
#include "missing.h"
|
||||
#include "msg.h"
|
||||
#include "port.h"
|
||||
|
|
@ -33,6 +34,8 @@
|
|||
#include "tmv.h"
|
||||
#include "util.h"
|
||||
|
||||
#define PORT_MAVE_LENGTH 10
|
||||
|
||||
#define PTP_VERSION 2
|
||||
|
||||
#define LOG_MIN_PDELAY_REQ_INTERVAL 2 /* allow PDelay_Req every 4 sec */
|
||||
|
|
@ -56,6 +59,8 @@ struct port {
|
|||
UInteger16 sync;
|
||||
} seqnum;
|
||||
struct tmtab tmtab;
|
||||
tmv_t peer_delay;
|
||||
struct mave *avg_delay;
|
||||
/* portDS */
|
||||
struct port_defaults pod;
|
||||
struct PortIdentity portIdentity;
|
||||
|
|
@ -346,11 +351,59 @@ static void port_synchronize(struct port *p,
|
|||
}
|
||||
}
|
||||
|
||||
static int port_pdelay_request(struct port *p)
|
||||
{
|
||||
struct ptp_message *msg;
|
||||
int cnt, pdulen;
|
||||
|
||||
msg = msg_allocate();
|
||||
if (!msg)
|
||||
return -1;
|
||||
memset(msg, 0, sizeof(*msg));
|
||||
|
||||
pdulen = sizeof(struct pdelay_req_msg);
|
||||
msg->hwts.type = p->timestamping;
|
||||
|
||||
msg->header.tsmt = PDELAY_REQ;
|
||||
msg->header.ver = PTP_VERSION;
|
||||
msg->header.messageLength = pdulen;
|
||||
msg->header.domainNumber = clock_domain_number(p->clock);
|
||||
msg->header.sourcePortIdentity = p->portIdentity;
|
||||
msg->header.sequenceId = p->seqnum.delayreq++;
|
||||
msg->header.control = CTL_OTHER;
|
||||
msg->header.logMessageInterval = 0x7f;
|
||||
|
||||
if (msg_pre_send(msg))
|
||||
goto out;
|
||||
|
||||
cnt = transport_send(p->trp, &p->fda, 1, msg, pdulen, &msg->hwts);
|
||||
if (cnt <= 0) {
|
||||
pr_err("port %hu: send peer delay request failed", portnum(p));
|
||||
goto out;
|
||||
}
|
||||
if (msg_sots_missing(msg)) {
|
||||
pr_err("missing timestamp on transmitted peer delay request");
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (p->peer_delay_req)
|
||||
msg_put(p->peer_delay_req);
|
||||
|
||||
p->peer_delay_req = msg;
|
||||
return 0;
|
||||
out:
|
||||
msg_put(msg);
|
||||
return -1;
|
||||
}
|
||||
|
||||
static int port_delay_request(struct port *p)
|
||||
{
|
||||
struct ptp_message *msg;
|
||||
int cnt, pdulen;
|
||||
|
||||
if (p->delayMechanism == DM_P2P)
|
||||
return port_pdelay_request(p);
|
||||
|
||||
msg = msg_allocate();
|
||||
if (!msg)
|
||||
return -1;
|
||||
|
|
@ -813,6 +866,192 @@ static void process_follow_up(struct port *p, struct ptp_message *m)
|
|||
syn->header.correction, m->header.correction);
|
||||
}
|
||||
|
||||
static int process_pdelay_req(struct port *p, struct ptp_message *m)
|
||||
{
|
||||
struct ptp_message *rsp, *fup;
|
||||
int cnt, err = -1, rsp_len, fup_len;
|
||||
|
||||
if (p->delayMechanism == DM_E2E) {
|
||||
pr_warning("port %hu: pdelay_req on E2E port", portnum(p));
|
||||
return -1;
|
||||
}
|
||||
if (p->delayMechanism == DM_AUTO) {
|
||||
pr_info("port %hu: peer detected, switch to P2P", portnum(p));
|
||||
p->delayMechanism = DM_P2P;
|
||||
}
|
||||
|
||||
rsp = msg_allocate();
|
||||
if (!rsp)
|
||||
return -1;
|
||||
fup = msg_allocate();
|
||||
if (!fup) {
|
||||
msg_put(rsp);
|
||||
return -1;
|
||||
}
|
||||
memset(rsp, 0, sizeof(*rsp));
|
||||
memset(fup, 0, sizeof(*fup));
|
||||
|
||||
rsp_len = sizeof(struct pdelay_resp_msg);
|
||||
rsp->hwts.type = p->timestamping;
|
||||
|
||||
rsp->header.tsmt = PDELAY_RESP;
|
||||
rsp->header.ver = PTP_VERSION;
|
||||
rsp->header.messageLength = rsp_len;
|
||||
rsp->header.domainNumber = m->header.domainNumber;
|
||||
rsp->header.sourcePortIdentity = p->portIdentity;
|
||||
rsp->header.sequenceId = m->header.sequenceId;
|
||||
rsp->header.control = CTL_OTHER;
|
||||
rsp->header.logMessageInterval = 0x7f;
|
||||
|
||||
rsp->header.flagField[0] |= TWO_STEP;
|
||||
|
||||
/*
|
||||
* NB - We do not have any fraction nanoseconds for the correction
|
||||
* fields, neither in the response or the follow up.
|
||||
*/
|
||||
ts_to_timestamp(&m->hwts.ts, &rsp->pdelay_resp.requestReceiptTimestamp);
|
||||
rsp->pdelay_resp.requestingPortIdentity = m->header.sourcePortIdentity;
|
||||
|
||||
fup_len = sizeof(struct pdelay_resp_fup_msg);
|
||||
fup->hwts.type = p->timestamping;
|
||||
|
||||
fup->header.tsmt = PDELAY_RESP_FOLLOW_UP;
|
||||
fup->header.ver = PTP_VERSION;
|
||||
fup->header.messageLength = fup_len;
|
||||
fup->header.domainNumber = m->header.domainNumber;
|
||||
fup->header.correction = m->header.correction;
|
||||
fup->header.sourcePortIdentity = p->portIdentity;
|
||||
fup->header.sequenceId = m->header.sequenceId;
|
||||
fup->header.control = CTL_OTHER;
|
||||
fup->header.logMessageInterval = 0x7f;
|
||||
|
||||
fup->pdelay_resp_fup.requestingPortIdentity = m->header.sourcePortIdentity;
|
||||
|
||||
if (msg_pre_send(rsp))
|
||||
goto out;
|
||||
|
||||
cnt = transport_send(p->trp, &p->fda, 1, rsp, rsp_len, &rsp->hwts);
|
||||
if (cnt <= 0) {
|
||||
pr_err("port %hu: send peer delay response failed", portnum(p));
|
||||
goto out;
|
||||
}
|
||||
if (msg_sots_missing(rsp)) {
|
||||
pr_err("missing timestamp on transmitted peer delay response");
|
||||
goto out;
|
||||
}
|
||||
|
||||
ts_to_timestamp(&rsp->hwts.ts,
|
||||
&fup->pdelay_resp_fup.responseOriginTimestamp);
|
||||
|
||||
if (msg_pre_send(fup))
|
||||
goto out;
|
||||
|
||||
cnt = transport_send(p->trp, &p->fda, 0, fup, fup_len, &rsp->hwts);
|
||||
if (cnt <= 0) {
|
||||
pr_err("port %hu: send pdelay_resp_fup failed", portnum(p));
|
||||
goto out;
|
||||
}
|
||||
err = 0;
|
||||
out:
|
||||
msg_put(rsp);
|
||||
msg_put(fup);
|
||||
return err;
|
||||
}
|
||||
|
||||
static void port_peer_delay(struct port *p)
|
||||
{
|
||||
tmv_t c1, c2, t1, t2, t3, t4, pd;
|
||||
struct ptp_message *req = p->peer_delay_req;
|
||||
struct ptp_message *rsp = p->peer_delay_resp;
|
||||
struct ptp_message *fup = p->peer_delay_fup;
|
||||
|
||||
/* Check for response, validate port and sequence number. */
|
||||
|
||||
if (!rsp)
|
||||
return;
|
||||
|
||||
if (!pid_eq(&rsp->pdelay_resp.requestingPortIdentity, &p->portIdentity))
|
||||
return;
|
||||
|
||||
if (rsp->header.sequenceId != ntohs(req->header.sequenceId))
|
||||
return;
|
||||
|
||||
// TODO - add asymmetry value to correctionField.
|
||||
|
||||
t1 = timespec_to_tmv(req->hwts.ts);
|
||||
t4 = timespec_to_tmv(rsp->hwts.ts);
|
||||
c1 = correction_to_tmv(rsp->header.correction);
|
||||
|
||||
/* Process one-step response immediately. */
|
||||
if (one_step(rsp)) {
|
||||
t2 = tmv_zero();
|
||||
t3 = tmv_zero();
|
||||
c2 = tmv_zero();
|
||||
goto calc;
|
||||
}
|
||||
|
||||
/* Check for follow up, validate port and sequence number. */
|
||||
|
||||
if (!fup)
|
||||
return;
|
||||
|
||||
if (!pid_eq(&fup->pdelay_resp_fup.requestingPortIdentity, &p->portIdentity))
|
||||
return;
|
||||
|
||||
if (fup->header.sequenceId != rsp->header.sequenceId)
|
||||
return;
|
||||
|
||||
if (!pid_eq(&fup->header.sourcePortIdentity,
|
||||
&rsp->header.sourcePortIdentity))
|
||||
return;
|
||||
|
||||
/* Process follow up response. */
|
||||
t2 = timestamp_to_tmv(rsp->ts.pdu);
|
||||
t3 = timestamp_to_tmv(fup->ts.pdu);
|
||||
c2 = correction_to_tmv(fup->header.correction);
|
||||
calc:
|
||||
pd = tmv_sub(tmv_sub(t4, t1), tmv_sub(t3, t2));
|
||||
pd = tmv_sub(pd, c1);
|
||||
pd = tmv_sub(pd, c2);
|
||||
pd = tmv_div(pd, 2);
|
||||
|
||||
p->peer_delay = mave_accumulate(p->avg_delay, pd);
|
||||
|
||||
pr_debug("pdelay %hu %10lld %10lld", portnum(p), p->peer_delay, pd);
|
||||
|
||||
if (p->state == PS_UNCALIBRATED || p->state == PS_SLAVE) {
|
||||
clock_peer_delay(p->clock, p->peer_delay);
|
||||
}
|
||||
}
|
||||
|
||||
static void process_pdelay_resp(struct port *p, struct ptp_message *m)
|
||||
{
|
||||
if (!p->peer_delay_req)
|
||||
return;
|
||||
|
||||
if (p->peer_delay_resp) {
|
||||
// TODO - make sure peer source port is same.
|
||||
msg_put(p->peer_delay_resp);
|
||||
}
|
||||
|
||||
msg_get(m);
|
||||
p->peer_delay_resp = m;
|
||||
port_peer_delay(p);
|
||||
}
|
||||
|
||||
static void process_pdelay_resp_fup(struct port *p, struct ptp_message *m)
|
||||
{
|
||||
if (!p->peer_delay_req)
|
||||
return;
|
||||
|
||||
if (p->peer_delay_fup)
|
||||
msg_put(p->peer_delay_fup);
|
||||
|
||||
msg_get(m);
|
||||
p->peer_delay_fup = m;
|
||||
port_peer_delay(p);
|
||||
}
|
||||
|
||||
static void process_sync(struct port *p, struct ptp_message *m)
|
||||
{
|
||||
struct ptp_message *fup;
|
||||
|
|
@ -869,6 +1108,7 @@ void port_close(struct port *p)
|
|||
port_disable(p);
|
||||
}
|
||||
transport_destroy(p->trp);
|
||||
mave_destroy(p->avg_delay);
|
||||
free(p);
|
||||
}
|
||||
|
||||
|
|
@ -961,6 +1201,25 @@ void port_dispatch(struct port *p, enum fsm_event event, int mdiff)
|
|||
port_set_delay_tmo(p);
|
||||
break;
|
||||
};
|
||||
if (p->delayMechanism == DM_P2P) {
|
||||
switch (next) {
|
||||
case PS_INITIALIZING:
|
||||
case PS_FAULTY:
|
||||
case PS_DISABLED:
|
||||
break;
|
||||
case PS_LISTENING:
|
||||
case PS_PRE_MASTER:
|
||||
case PS_MASTER:
|
||||
case PS_GRAND_MASTER:
|
||||
case PS_PASSIVE:
|
||||
port_set_delay_tmo(p);
|
||||
break;
|
||||
case PS_UNCALIBRATED:
|
||||
case PS_SLAVE:
|
||||
/*already set above*/
|
||||
break;
|
||||
};
|
||||
}
|
||||
p->state = next;
|
||||
}
|
||||
|
||||
|
|
@ -1028,7 +1287,10 @@ enum fsm_event port_event(struct port *p, int fd_index)
|
|||
process_delay_req(p, msg);
|
||||
break;
|
||||
case PDELAY_REQ:
|
||||
process_pdelay_req(p, msg);
|
||||
break;
|
||||
case PDELAY_RESP:
|
||||
process_pdelay_resp(p, msg);
|
||||
break;
|
||||
case FOLLOW_UP:
|
||||
process_follow_up(p, msg);
|
||||
|
|
@ -1037,6 +1299,7 @@ enum fsm_event port_event(struct port *p, int fd_index)
|
|||
process_delay_resp(p, msg);
|
||||
break;
|
||||
case PDELAY_RESP_FOLLOW_UP:
|
||||
process_pdelay_resp_fup(p, msg);
|
||||
break;
|
||||
case ANNOUNCE:
|
||||
if (process_announce(p, msg))
|
||||
|
|
@ -1080,6 +1343,14 @@ struct port *port_open(struct port_defaults *pod,
|
|||
p->delayMechanism = dm;
|
||||
p->versionNumber = PTP_VERSION;
|
||||
|
||||
p->avg_delay = mave_create(PORT_MAVE_LENGTH);
|
||||
if (!p->avg_delay) {
|
||||
pr_err("Failed to create moving average");
|
||||
transport_destroy(p->trp);
|
||||
free(p);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue