From 05e49d81ced009642ee418718e185cb93c4b6d4a Mon Sep 17 00:00:00 2001 From: Aurelio Colosimo Date: Tue, 17 Jan 2012 10:25:10 +0100 Subject: [PATCH] handling of recv packet timestamp --- arch-bare-linux/bare-linux.h | 3 ++- arch-bare-linux/bare-socket.c | 5 +++-- arch-bare-linux/main-loop.c | 7 ++++++- arch-gnu-linux/main-loop.c | 7 ++++++- arch-gnu-linux/posix-socket.c | 5 +++-- arch-gnu-linux/posix.h | 3 ++- arch-spec/spec-socket.c | 5 +++-- arch-spec/spec.h | 3 ++- include/pptp/pptp.h | 7 ++++++- proto-standard/arith.c | 6 ++++++ proto-standard/common-fun.c | 15 ++++++++++++--- proto-standard/common-fun.h | 6 +++--- proto-standard/state-listening.c | 3 +-- proto-standard/state-master.c | 22 ++++++++++++---------- proto-standard/state-passive.c | 5 ++--- proto-standard/state-slave.c | 20 +++++++++++--------- proto-standard/state-uncalibrated.c | 3 +-- 17 files changed, 81 insertions(+), 44 deletions(-) diff --git a/arch-bare-linux/bare-linux.h b/arch-bare-linux/bare-linux.h index fafa3c7..cbefbcc 100644 --- a/arch-bare-linux/bare-linux.h +++ b/arch-bare-linux/bare-linux.h @@ -7,7 +7,8 @@ */ extern int bare_open_ch(struct pp_instance *ppi, char *name); -extern int bare_recv_packet(struct pp_instance *ppi, void *pkt, int len); +extern int bare_recv_packet(struct pp_instance *ppi, void *pkt, int len, + TimeInternal *t); extern int bare_send_packet(struct pp_instance *ppi, void *pkt, int len, int chtype); diff --git a/arch-bare-linux/bare-socket.c b/arch-bare-linux/bare-socket.c index db682d1..494ce75 100644 --- a/arch-bare-linux/bare-socket.c +++ b/arch-bare-linux/bare-socket.c @@ -8,7 +8,8 @@ #include "bare-linux.h" /* FIXME: which socket we receive and send with? */ -int bare_recv_packet(struct pp_instance *ppi, void *pkt, int len) +int bare_recv_packet(struct pp_instance *ppi, void *pkt, int len, + TimeInternal *t) { return sys_recv(NP(ppi)->ch[PP_NP_GEN].fd, pkt, len, 0); } @@ -18,7 +19,7 @@ int bare_send_packet(struct pp_instance *ppi, void *pkt, int len, int chtype) return sys_send(NP(ppi)->ch[chtype].fd, pkt, len, 0); } -int pp_recv_packet(struct pp_instance *ppi, void *pkt, int len) +int pp_recv_packet(struct pp_instance *ppi, void *pkt, int len, TimeInternal *t) __attribute__((alias("bare_recv_packet"))); int pp_send_packet(struct pp_instance *ppi, void *pkt, int len, int chtype) __attribute__((alias("bare_send_packet"))); diff --git a/arch-bare-linux/main-loop.c b/arch-bare-linux/main-loop.c index de8cb2a..f382a96 100644 --- a/arch-bare-linux/main-loop.c +++ b/arch-bare-linux/main-loop.c @@ -28,6 +28,8 @@ void bare_main_loop(struct pp_instance *ppi) { int delay_ms; + set_TimeInternal(ppi->last_rcv_time, 0, 0); + /* * The main loop here is based on select. While we are not * doing anything else but the protocol, this allows extra stuff @@ -69,7 +71,10 @@ void bare_main_loop(struct pp_instance *ppi) * * FIXME: we don't know which socket to receive from */ - i = bare_recv_packet(ppi, packet, sizeof(packet)); + i = bare_recv_packet(ppi, packet, sizeof(packet), + &ppi->last_rcv_time); + ppi->last_rcv_time.seconds += DSPRO(ppi)->currentUtcOffset; + /* FIXME: PP_PROTO_NR is a legacy number */ if (((struct bare_ethhdr *)packet)->h_proto != htons(PP_PROTO_NR)) diff --git a/arch-gnu-linux/main-loop.c b/arch-gnu-linux/main-loop.c index 95b5432..4e7416a 100644 --- a/arch-gnu-linux/main-loop.c +++ b/arch-gnu-linux/main-loop.c @@ -18,6 +18,8 @@ void posix_main_loop(struct pp_instance *ppi) { int delay_ms; + set_TimeInternal(&ppi->last_rcv_time, 0, 0); + /* * The main loop here is based on select. While we are not * doing anything else but the protocol, this allows extra stuff @@ -44,7 +46,10 @@ void posix_main_loop(struct pp_instance *ppi) * We got a packet. If it's not ours, continue consuming * the pending timeout */ - i = posix_recv_packet(ppi, packet, sizeof(packet)); + i = posix_recv_packet(ppi, packet, sizeof(packet), + &ppi->last_rcv_time); + + ppi->last_rcv_time.seconds += DSPRO(ppi)->currentUtcOffset; if (i < PP_PACKET_SIZE) { delay_ms = -1; diff --git a/arch-gnu-linux/posix-socket.c b/arch-gnu-linux/posix-socket.c index e12dd5b..3234bf0 100644 --- a/arch-gnu-linux/posix-socket.c +++ b/arch-gnu-linux/posix-socket.c @@ -22,7 +22,8 @@ static int ch_check_stat = 0; /* Receive and send is *not* so trivial */ -int posix_recv_packet(struct pp_instance *ppi, void *pkt, int len) +int posix_recv_packet(struct pp_instance *ppi, void *pkt, int len, + TimeInternal *t) { struct pp_channel *ch1 = NULL, *ch2 = NULL; @@ -56,7 +57,7 @@ int posix_send_packet(struct pp_instance *ppi, void *pkt, int len, int chtype) return send(NP(ppi)->ch[chtype].fd, pkt, len, 0); } -int pp_recv_packet(struct pp_instance *ppi, void *pkt, int len) +int pp_recv_packet(struct pp_instance *ppi, void *pkt, int len, TimeInternal *t) __attribute__((alias("posix_recv_packet"))); int pp_send_packet(struct pp_instance *ppi, void *pkt, int len, int chtype) __attribute__((alias("posix_send_packet"))); diff --git a/arch-gnu-linux/posix.h b/arch-gnu-linux/posix.h index 9cd5503..fc5f9ef 100644 --- a/arch-gnu-linux/posix.h +++ b/arch-gnu-linux/posix.h @@ -15,7 +15,8 @@ extern int posix_net_check_pkt(struct pp_instance *ppi, int delay_ms); extern int posix_open_ch(struct pp_instance *ppi, char *name); -extern int posix_recv_packet(struct pp_instance *ppi, void *pkt, int len); +extern int posix_recv_packet(struct pp_instance *ppi, void *pkt, int len, + TimeInternal *t); extern int posix_send_packet(struct pp_instance *ppi, void *pkt, int len, int chtype); diff --git a/arch-spec/spec-socket.c b/arch-spec/spec-socket.c index ede1610..49e98d3 100644 --- a/arch-spec/spec-socket.c +++ b/arch-spec/spec-socket.c @@ -23,7 +23,8 @@ int spec_open_ch(struct pp_instance *ppi) } /* To receive and send packets, we call the minic low-level stuff */ -int spec_recv_packet(struct pp_instance *ppi, void *pkt, int len) +int spec_recv_packet(struct pp_instance *ppi, void *pkt, int len, + TimeInternal *t) { static int led; @@ -41,7 +42,7 @@ int spec_send_packet(struct pp_instance *ppi, void *pkt, int len, int chtype) return minic_tx_frame(pkt, pkt + 14, len, NULL); } -int pp_recv_packet(struct pp_instance *ppi, void *pkt, int len) +int pp_recv_packet(struct pp_instance *ppi, void *pkt, int len, TimeInternal *t) __attribute__((alias("spec_recv_packet"))); int pp_send_packet(struct pp_instance *ppi, void *pkt, int len, int chtype) __attribute__((alias("spec_send_packet"))); diff --git a/arch-spec/spec.h b/arch-spec/spec.h index feafc12..dac94de 100644 --- a/arch-spec/spec.h +++ b/arch-spec/spec.h @@ -7,7 +7,8 @@ */ extern int spec_open_ch(struct pp_instance *ppi); -extern int spec_recv_packet(struct pp_instance *ppi, void *pkt, int len); +extern int spec_recv_packet(struct pp_instance *ppi, void *pkt, int len, + TimeInternal *t); extern int spec_send_packet(struct pp_instance *ppi, void *pkt, int len); extern void spec_main_loop(struct pp_instance *ppi); diff --git a/include/pptp/pptp.h b/include/pptp/pptp.h index cf680b2..ae66a2e 100644 --- a/include/pptp/pptp.h +++ b/include/pptp/pptp.h @@ -134,6 +134,9 @@ struct pp_instance { Octet *buf_in; /* FIXME really useful? Probably not*/ TimeInternal sync_receive_time; UInteger16 recv_sync_sequence_id; + + TimeInternal last_rcv_time; /* used to store timestamp retreived from + * received packet */ TimeInternal last_sync_corr_field; TimeInternal last_pdelay_req_corr_field; TimeInternal last_pdelay_resp_corr_field; @@ -188,7 +191,8 @@ extern int pp_parse_cmdline(struct pp_instance *ppi, int argc, char **argv); /* Network stuff */ extern int pp_net_init(struct pp_instance *ppi); extern int pp_net_shutdown(struct pp_instance *ppi); -extern int pp_recv_packet(struct pp_instance *ppi, void *pkt, int len); +extern int pp_recv_packet(struct pp_instance *ppi, void *pkt, int len, + TimeInternal *t); extern int pp_send_packet(struct pp_instance *ppi, void *pkt, int len, int chtype); /* chtype: PP_NP_GEN || PP_NP_EVT */ @@ -265,6 +269,7 @@ extern int from_TimeInternal(TimeInternal *internal, Timestamp *external); extern int to_TimeInternal(TimeInternal *internal, Timestamp *external); extern void add_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y); extern void sub_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y); +extern void set_TimeInternal(TimeInternal *t, Integer32 s, Integer32 ns); /* Get and Set system timestamp */ extern void pp_get_tstamp(TimeInternal *t); diff --git a/proto-standard/arith.c b/proto-standard/arith.c index f1c129a..77c5454 100644 --- a/proto-standard/arith.c +++ b/proto-standard/arith.c @@ -93,3 +93,9 @@ void sub_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y) normalize_TimeInternal(r); } + +void set_TimeInternal(TimeInternal *t, Integer32 s, Integer32 ns) +{ + t->seconds = s; + t->nanoseconds = ns; +} diff --git a/proto-standard/common-fun.c b/proto-standard/common-fun.c index bd3c07d..d418b53 100644 --- a/proto-standard/common-fun.c +++ b/proto-standard/common-fun.c @@ -160,8 +160,9 @@ int st_com_slave_handle_announce(struct pp_instance *ppi, unsigned char *buf, } int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf, - int len, TimeInternal *time) + int len) { + TimeInternal *time; TimeInternal origin_tstamp; TimeInternal correction_field; MsgHeader *hdr = &ppi->msg_tmp_header; @@ -172,6 +173,8 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf, if (ppi->is_from_self) return 0; + time = &ppi->last_rcv_time; + if (ppi->is_from_cur_par) { ppi->sync_receive_time.seconds = time->seconds; ppi->sync_receive_time.nanoseconds = time->nanoseconds; @@ -262,8 +265,9 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf, int st_com_handle_pdelay_req(struct pp_instance *ppi, unsigned char *buf, - int len, TimeInternal *time) + int len) { + TimeInternal *time; MsgHeader *hdr = &ppi->msg_tmp_header; if (len < PP_PDELAY_REQ_LENGTH) @@ -272,6 +276,8 @@ int st_com_handle_pdelay_req(struct pp_instance *ppi, unsigned char *buf, if (ppi->rt_opts->e2e_mode) return 0; + time = &ppi->last_rcv_time; + if (ppi->is_from_self) { /* Get sending timestamp from IP stack * with So_TIMESTAMP */ @@ -313,14 +319,17 @@ int st_com_master_handle_announce(struct pp_instance *ppi, unsigned char *buf, } int st_com_master_handle_sync(struct pp_instance *ppi, unsigned char *buf, - int len, TimeInternal *time) + int len) { + TimeInternal *time; if (len < PP_SYNC_LENGTH) return -1; if (!ppi->is_from_self) return 0; + time = &ppi->last_rcv_time; + /* Add latency */ add_TimeInternal(time, time, &ppi->rt_opts->outbound_latency); msg_issue_followup(ppi, time); diff --git a/proto-standard/common-fun.h b/proto-standard/common-fun.h index 2916e63..4ddee67 100644 --- a/proto-standard/common-fun.h +++ b/proto-standard/common-fun.h @@ -25,13 +25,13 @@ int st_com_master_handle_announce(struct pp_instance *ppi, unsigned char *buf, int len); int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf, - int len, TimeInternal *time); + int len); int st_com_master_handle_sync(struct pp_instance *ppi, unsigned char *buf, - int len, TimeInternal *time); + int len); int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf, int len); int st_com_handle_pdelay_req(struct pp_instance *ppi, unsigned char *buf, - int len, TimeInternal *time); + int len); diff --git a/proto-standard/state-listening.c b/proto-standard/state-listening.c index ff12b2f..1a0eaf4 100644 --- a/proto-standard/state-listening.c +++ b/proto-standard/state-listening.c @@ -8,7 +8,6 @@ int pp_listening(struct pp_instance *ppi, unsigned char *pkt, int plen) { - TimeInternal time; /* TODO: handle it, see handle(...) in protocol.c */ int e = 0; /* error var, to check errors in msg handling */ if (ppi->is_new_state) @@ -24,7 +23,7 @@ int pp_listening(struct pp_instance *ppi, unsigned char *pkt, int plen) break; case PPM_SYNC: - e = st_com_master_handle_sync(ppi, pkt, plen, &time); + e = st_com_master_handle_sync(ppi, pkt, plen); break; default: diff --git a/proto-standard/state-master.c b/proto-standard/state-master.c index 8b8584c..e6232fb 100644 --- a/proto-standard/state-master.c +++ b/proto-standard/state-master.c @@ -9,7 +9,7 @@ int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen) { - TimeInternal time; /* TODO: handle it, see handle(...) in protocol.c */ + TimeInternal *time; TimeInternal req_rec_tstamp; TimeInternal correction_field; TimeInternal resp_orig_tstamp; @@ -17,6 +17,8 @@ int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen) int e = 0; /* error var, to check errors in msg handling */ MsgHeader *hdr = &ppi->msg_tmp_header; + time = &ppi->last_rcv_time; + if (ppi->is_new_state) { pp_timer_start(1 << DSPOR(ppi)->logSyncInterval, ppi->timers[PP_TIMER_SYNC]); @@ -64,16 +66,16 @@ int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen) break; case PPM_SYNC: - e = st_com_master_handle_sync(ppi, pkt, plen, &time); + e = st_com_master_handle_sync(ppi, pkt, plen); break; case PPM_DELAY_REQ: msg_copy_header(&ppi->delay_req_hdr, hdr); - msg_issue_delay_resp(ppi, &time); + msg_issue_delay_resp(ppi, time); break; case PPM_PDELAY_REQ: - e = st_com_handle_pdelay_req(ppi, pkt, plen, &time); + e = st_com_handle_pdelay_req(ppi, pkt, plen); break; case PPM_PDELAY_RESP: @@ -83,10 +85,10 @@ int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen) if (ppi->is_from_self) { /*Add latency*/ - add_TimeInternal(&time, &time, + add_TimeInternal(time, time, &ppi->rt_opts->outbound_latency); - e = msg_issue_pdelay_resp_follow_up(ppi,&time); + e = msg_issue_pdelay_resp_follow_up(ppi, time); break; } msg_unpack_pdelay_resp(pkt, &ppi->msg_tmp.presp); @@ -104,9 +106,9 @@ int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen) /* Two Step Clock */ /* Store t4 (Fig 35) */ ppi->pdelay_resp_receive_time.seconds = - time.seconds; + time->seconds; ppi->pdelay_resp_receive_time.nanoseconds = - time.nanoseconds; + time->nanoseconds; /* Store t2 (Fig 35) */ to_TimeInternal(&req_rec_tstamp, &ppi->msg_tmp.presp. @@ -127,9 +129,9 @@ int pp_master(struct pp_instance *ppi, unsigned char *pkt, int plen) /* One step Clock */ /* Store t4 (Fig 35)*/ ppi->pdelay_resp_receive_time.seconds = - time.seconds; + time->seconds; ppi->pdelay_resp_receive_time.nanoseconds = - time.nanoseconds; + time->nanoseconds; int64_to_TimeInternal( hdr->correctionfield, diff --git a/proto-standard/state-passive.c b/proto-standard/state-passive.c index e21db9f..9467928 100644 --- a/proto-standard/state-passive.c +++ b/proto-standard/state-passive.c @@ -8,7 +8,6 @@ int pp_passive(struct pp_instance *ppi, unsigned char *pkt, int plen) { - TimeInternal time; /* TODO: handle it, see handle(...) in protocol.c */ int e = 0; /* error var, to check errors in msg handling */ if (ppi->is_new_state) { @@ -28,11 +27,11 @@ int pp_passive(struct pp_instance *ppi, unsigned char *pkt, int plen) break; case PPM_PDELAY_REQ: - e = st_com_handle_pdelay_req(ppi, pkt, plen, &time); + e = st_com_handle_pdelay_req(ppi, pkt, plen); break; case PPM_SYNC: - e = st_com_master_handle_sync(ppi, pkt, plen, &time); + e = st_com_master_handle_sync(ppi, pkt, plen); break; default: diff --git a/proto-standard/state-slave.c b/proto-standard/state-slave.c index 136acdb..c54e905 100644 --- a/proto-standard/state-slave.c +++ b/proto-standard/state-slave.c @@ -10,12 +10,14 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen) { int e = 0; /* error var, to check errors in msg handling */ - TimeInternal time; /* TODO: handle it, see handle(...) in protocol.c */ + TimeInternal *time; TimeInternal req_rec_tstamp; TimeInternal correction_field; TimeInternal resp_orig_tstamp; MsgHeader *hdr = &ppi->msg_tmp_header; + time = &ppi->last_rcv_time; + if (ppi->is_new_state) { pp_init_clock(ppi); @@ -50,7 +52,7 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen) break; case PPM_SYNC: - e = st_com_slave_handle_sync(ppi, pkt, plen, &time); + e = st_com_slave_handle_sync(ppi, pkt, plen); break; case PPM_FOLLOW_UP: @@ -66,9 +68,9 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen) * with So_TIMESTAMP */ ppi->delay_req_send_time.seconds = - time.seconds; + time->seconds; ppi->delay_req_send_time.nanoseconds = - time.nanoseconds; + time->nanoseconds; /* Add latency */ add_TimeInternal(&ppi->delay_req_send_time, @@ -125,7 +127,7 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen) break; case PPM_PDELAY_REQ: - e = st_com_handle_pdelay_req(ppi, pkt, plen, &time); + e = st_com_handle_pdelay_req(ppi, pkt, plen); break; case PPM_PDELAY_RESP: @@ -160,9 +162,9 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen) /* Two Step Clock */ /* Store t4 (Fig 35) */ ppi->pdelay_resp_receive_time.seconds = - time.seconds; + time->seconds; ppi->pdelay_resp_receive_time.nanoseconds = - time.nanoseconds; + time->nanoseconds; /* Store t2 (Fig 35) */ to_TimeInternal(&req_rec_tstamp, &ppi->msg_tmp.presp. @@ -182,9 +184,9 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen) /* One step Clock */ /* Store t4 (Fig 35) */ ppi->pdelay_resp_receive_time.seconds = - time.seconds; + time->seconds; ppi->pdelay_resp_receive_time.nanoseconds = - time.nanoseconds; + time->nanoseconds; int64_to_TimeInternal(hdr->correctionfield, &correction_field); diff --git a/proto-standard/state-uncalibrated.c b/proto-standard/state-uncalibrated.c index a459328..c047d74 100644 --- a/proto-standard/state-uncalibrated.c +++ b/proto-standard/state-uncalibrated.c @@ -9,7 +9,6 @@ int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen) { int e = 0; /* error var, to check errors in msg handling */ - TimeInternal time; /* TODO: handle it, see handle(...) in protocol.c */ switch (ppi->msg_tmp_header.messageType) { @@ -18,7 +17,7 @@ int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen) break; case PPM_SYNC: - e = st_com_slave_handle_sync(ppi, pkt, plen, &time); + e = st_com_slave_handle_sync(ppi, pkt, plen); break; case PPM_FOLLOW_UP: -- 2.18.1