Skip to content

P
PPSi
Commit 17df60f4 authored by Alessandro Rubini's avatar Alessandro Rubini
Browse files
Options

time: fix wrs 

Signed-off-by: Alessandro Rubini's avatarAlessandro Rubini <rubini@gnudd.com>
parent 24fe3621
Hide whitespace changes
Inline Side-by-side
Showing with 87 additions and 79 deletions
time-wrs/wrs-socket.c
View file @ 17df60f4
......@@ -100,6 +100,15 @@ static inline int inside_range(int min, int max, int x)
return (x<=max || x>=min);
}
static char *fmt_time(struct pp_time *t)
{
static char buf[64];
sprintf(buf, "(correct %i) %9li.%09li",
!is_incorrect(t), (long)t->secs,
(long)(t->scaled_nsecs >> 16));
return buf;
}
static void update_dmtd(struct wrs_socket *s, struct pp_instance *ppi)
{
struct hal_port_state *p;
......@@ -116,14 +125,19 @@ static void update_dmtd(struct wrs_socket *s, struct pp_instance *ppi)
}
}
static void wrs_linearize_rx_timestamp(TimeInternal *ts,
/*
* Note: it looks like transition_point is always zero.
* Also, all calculations are ps here, but the timestamp is scaled_ns
* -- ARub 2016-10
*/
static void wrs_linearize_rx_timestamp(struct pp_time *ts,
int32_t dmtd_phase, int cntr_ahead, int transition_point,
int clock_period)
{
int trip_lo, trip_hi;
int phase;
phase = clock_period -1 -dmtd_phase;
phase = clock_period - 1 - dmtd_phase;
/* calculate the range within which falling edge timestamp is stable
* (no possible transitions) */
......@@ -140,25 +154,28 @@ static void wrs_linearize_rx_timestamp(TimeInternal *ts,
* "reliable" one. cntr_ahead will be 1 when the rising edge
* counter is 1 tick ahead of the falling edge counter */
ts->nanoseconds -= cntr_ahead ? (clock_period / 1000) : 0;
if (cntr_ahead)
ts->scaled_nsecs -= (clock_period / 1000LL) << 16;
/* check if the phase is before the counter transition value
* and eventually increase the counter by 1 to simulate a
* timestamp transition exactly at s->phase_transition
* DMTD phase value */
if(inside_range(trip_lo, transition_point, phase))
ts->nanoseconds += clock_period / 1000;
ts->scaled_nsecs += (clock_period / 1000LL) << 16;
}
ts->phase = phase - transition_point - 1;
if(ts->phase < 0) ts->phase += clock_period;
ts->phase = clock_period - 1 -ts->phase;
phase = phase - transition_point - 1;
if (phase < 0)
phase += clock_period;
phase = clock_period - 1 - phase;
ts->scaled_nsecs += (phase << 16) / 1000;
}
static int wrs_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
struct ethhdr *hdr = pkt;
struct wrs_socket *s;
......@@ -198,9 +215,6 @@ static int wrs_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
/* FIXME Check ptp-noposix, commit d34f56f: if sender mac check
* is required. Should be added here */
if (t)
t->correct = 0;
for (cmsg = CMSG_FIRSTHDR(&msg);
cmsg;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
......@@ -219,23 +233,18 @@ static int wrs_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int len,
if(sts && t)
{
int cntr_ahead = sts->hwtimeraw.tv_sec & 0x80000000 ? 1: 0;
t->nanoseconds = sts->hwtimeraw.tv_nsec;
t->seconds =
(uint64_t) sts->hwtimeraw.tv_sec & 0x7fffffff;
t->raw_ahead = cntr_ahead;
t->scaled_nsecs = (long long)sts->hwtimeraw.tv_nsec << 16;
t->secs = sts->hwtimeraw.tv_sec & 0x7fffffff;
update_dmtd(s, ppi);
if (!WR_DSPOR(ppi)->wrModeOn) {
/* for non-wr-mode any reported stamp is correct */
t->correct = 1;
goto drop;
}
if (s->dmtd_phase_valid)
{
if (s->dmtd_phase_valid) {
wrs_linearize_rx_timestamp(t, s->dmtd_phase,
cntr_ahead, s->phase_transition, s->clock_period);
t->correct = 1;
} else {
mark_incorrect(t);
}
}
......@@ -278,7 +287,7 @@ out:
}
static int wrs_net_recv(struct pp_instance *ppi, void *pkt, int len,
TimeInternal *t)
struct pp_time *t)
{
struct pp_channel *ch1, *ch2;
struct ethhdr *hdr = pkt;
......@@ -321,15 +330,13 @@ static int wrs_net_recv(struct pp_instance *ppi, void *pkt, int len,
if (ret < 0)
return ret;
pp_diag(ppi, time, 1, "recv stamp: (correct %i) %9li.%09li\n",
t->correct, (long)t->seconds,
(long)t->nanoseconds);
pp_diag(ppi, time, 1, "recv stamp: %s\n", fmt_time(t));
return ret;
}
/* Waits for the transmission timestamp and stores it in t (if not null). */
static void poll_tx_timestamp(struct pp_instance *ppi, void *pkt, int len,
struct wrs_socket *s, int fd, TimeInternal *t)
struct wrs_socket *s, int fd, struct pp_time *t)
{
char data[16384], *dataptr;
struct msghdr msg;
......@@ -357,7 +364,7 @@ static void poll_tx_timestamp(struct pp_instance *ppi, void *pkt, int len,
msg.msg_controllen = sizeof(control);
if (t) /* poison the stamp */
t->seconds = t->correct = 0;
mark_incorrect(t);
pfd.fd = fd;
pfd.events = POLLERR;
......@@ -415,23 +422,21 @@ static void poll_tx_timestamp(struct pp_instance *ppi, void *pkt, int len,
cmsg;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
void *dp = CMSG_DATA(cmsg);
void *dp = CMSG_DATA(cmsg);
if(cmsg->cmsg_level == SOL_PACKET
&& cmsg->cmsg_type == PACKET_TX_TIMESTAMP)
serr = (struct sock_extended_err *) dp;
if(cmsg->cmsg_level == SOL_PACKET
&& cmsg->cmsg_type == PACKET_TX_TIMESTAMP)
serr = (struct sock_extended_err *) dp;
if(cmsg->cmsg_level == SOL_SOCKET
&& cmsg->cmsg_type == SO_TIMESTAMPING)
sts = (struct scm_timestamping *) dp;
if(cmsg->cmsg_level == SOL_SOCKET
&& cmsg->cmsg_type == SO_TIMESTAMPING)
sts = (struct scm_timestamping *) dp;
if(sts && serr)
{
t->correct = 1;
t->phase = 0;
t->nanoseconds = sts->hwtimeraw.tv_nsec;
t->seconds = (uint64_t) sts->hwtimeraw.tv_sec & 0x7fffffff;
}
if(sts && serr) {
t->scaled_nsecs =
(long long)sts->hwtimeraw.tv_nsec << 16;
t->secs = sts->hwtimeraw.tv_sec & 0x7fffffff;
}
}
}
......@@ -443,7 +448,7 @@ static int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
struct ethhdr *hdr = pkt;
struct pp_vlanhdr *vhdr = pkt;
struct pp_channel *ch = ppi->ch + chtype;
TimeInternal *t = &ppi->last_snt_time;
struct pp_time *t = &ppi->last_snt_time;
int is_pdelay = pp_msgtype_info[msgtype].is_pdelay;
static uint16_t udpport[] = {
[PP_NP_GEN] = PP_GEN_PORT,
......@@ -492,9 +497,7 @@ static int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
if (pp_diag_allow(ppi, frames, 2))
dump_1588pkt("send: ", pkt, len, t, -1);
pp_diag(ppi, time, 1, "send stamp: (correct %i) %9li.%09li\n",
t->correct, (long)t->seconds,
(long)t->nanoseconds);
pp_diag(ppi, time, 1, "send stamp: %s\n", fmt_time(t));
return ret;
case PPSI_PROTO_VLAN:
......@@ -527,9 +530,7 @@ static int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
if (pp_diag_allow(ppi, frames, 2))
dump_1588pkt("send: ", pkt, len, t, ppi->peer_vid);
pp_diag(ppi, time, 1, "send stamp: (correct %i) %9li.%09li\n",
t->correct, (long)t->seconds,
(long)t->nanoseconds);
pp_diag(ppi, time, 1, "send stamp: %s\n", fmt_time(t));
return ret;
case PPSI_PROTO_UDP:
......@@ -553,9 +554,7 @@ static int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
if (pp_diag_allow(ppi, frames, 2))
dump_payloadpkt("send: ", pkt, len, t);
pp_diag(ppi, time, 1, "send stamp: (correct %i) %9li.%09li\n",
t->correct, (long)t->seconds,
(long)t->nanoseconds);
pp_diag(ppi, time, 1, "send stamp: %s\n", fmt_time(t));
return ret;
default:
......
time-wrs/wrs-time.c
View file @ 17df60f4
......@@ -153,10 +153,10 @@ int wrs_locking_poll(struct pp_instance *ppi, int grandmaster)
return WR_SPLL_READY;
}
/* This is a hack: we don't even have taih (struct pp_time come and save me) */
static int wrdate_get(TimeInternal *t)
/* This is a hack, but at least the year is 640bit clean */
static int wrdate_get(struct pp_time *t)
{
unsigned long tail, nsec, tmp;
unsigned long tail, taih, nsec, tmp1, tmp2;
static volatile uint32_t *pps;
int fd;
......@@ -173,18 +173,22 @@ static int wrdate_get(TimeInternal *t)
pps = mapaddr + 0x500; /* pps: 0x10010500 */
}
memset(t, 0, sizeof(*t));
do {
taih = pps[3];
tail = pps[2];
nsec = pps[1] * 16; /* we count a 62.5MHz */
tmp = pps[2];
} while(tmp != tail);
t->seconds = tail;
t->nanoseconds = nsec;
nsec = pps[1] * 16; /* we count at 62.5MHz */
tmp1 = pps[3];
tmp2 = pps[2];
} while((tmp1 != taih) || (tmp2 != tail));
t->secs = tail | ((uint64_t)taih << 32);
t->scaled_nsecs = nsec << 16;
return 0;
}
/* This is only used when the wrs is slave to a non-WR master */
static int wrs_time_get(struct pp_instance *ppi, TimeInternal *t)
static int wrs_time_get(struct pp_instance *ppi, struct pp_time *t)
{
hexp_pps_params_t p;
int cmd;
......@@ -204,9 +208,8 @@ static int wrs_time_get(struct pp_instance *ppi, TimeInternal *t)
/* FIXME Don't know whether p.current_phase_shift is to be assigned
* to t->phase or t->raw_phase. I ignore it, it's not useful here. */
t->seconds = p.current_sec;
t->nanoseconds = p.current_nsec;
t->correct = p.pps_valid;
t->secs = p.current_sec;
t->scaled_nsecs = (long long)p.current_nsec << 16;
if (!(pp_global_d_flags & PP_FLAG_NOTIMELOG))
pp_diag(ppi, time, 2, "%s: (valid %x) %9li.%09li\n", __func__,
......@@ -215,9 +218,9 @@ static int wrs_time_get(struct pp_instance *ppi, TimeInternal *t)
return rval;
}
static int wrs_time_set(struct pp_instance *ppi, TimeInternal *t)
static int wrs_time_set(struct pp_instance *ppi, const struct pp_time *t)
{
TimeInternal diff, now;
struct pp_time diff, now;
struct timex tx;
int tai_offset = 0;
int msec;
......@@ -238,11 +241,12 @@ static int wrs_time_set(struct pp_instance *ppi, TimeInternal *t)
* normal servo drives us). So get time to calc a rough difference.
*/
wrdate_get(&now);
sub_TimeInternal(&diff, t, &now);
diff = *t;
pp_time_sub(&diff, &now);
pp_diag(ppi, time, 1, "%s: (weird) %9li.%09li - delta %9li.%09li\n",
__func__,
(long)t->seconds, (long)t->nanoseconds,
(long)diff.seconds, (long)diff.nanoseconds);
(long)t->secs, (long)(t->scaled_nsecs >> 16),
(long)diff.secs, (long)(diff.scaled_nsecs >> 16));
/*
* We can adjust nanoseconds or seconds, but not both at the
......@@ -251,28 +255,28 @@ static int wrs_time_set(struct pp_instance *ppi, TimeInternal *t)
* servo will call us again later for the seconds part.
* Thus, we fall near, and can then trim frequency (hopefully).
*/
msec = diff.nanoseconds / 1000 / 1000;;
msec = (diff.scaled_nsecs >> 16) / 1000 / 1000;;
#define THRESHOLD_MS 20
if ((msec > THRESHOLD_MS && msec < (1000 - THRESHOLD_MS))
|| (msec < -THRESHOLD_MS && msec > (-1000 + THRESHOLD_MS))) {
pp_diag(ppi, time, 1, "%s: adjusting nanoseconds: %li\n",
__func__, (long)diff.nanoseconds);
diff.seconds = 0;
__func__, (long)(diff.scaled_nsecs >> 16));
diff.secs = 0;
} else {
diff.nanoseconds = 0;
diff.scaled_nsecs = 0;
if (msec > 500)
diff.seconds++;
diff.secs++;
if (msec < -500)
diff.seconds--;
diff.secs--;
pp_diag(ppi, time, 1, "%s: adjusting seconds: %li\n",
__func__, (long)diff.seconds);
__func__, (long)diff.secs);
}
wrs_adjust_counters(diff.seconds, diff.nanoseconds);
wrs_adjust_counters(diff.secs, diff.scaled_nsecs >> 16);
/* If WR time is unrelated to real-world time, we are done. */
if (t->seconds < 1420730822 /* "now" as I write this */)
if (t->secs < 1420730822 /* "now" as I write this */)
return 0;
/*
......@@ -288,8 +292,13 @@ static int wrs_time_set(struct pp_instance *ppi, TimeInternal *t)
*/
tai_offset = *((int *)(&tx.stbcnt) + 1);
}
t->seconds -= tai_offset;
unix_time_ops.set(ppi, t);
{
struct pp_time utc = *t; /* t is "const". uff.... */
utc.secs -= tai_offset;
unix_time_ops.set(ppi, &utc);
}
return 0;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment