Commit 987288dc authored by Aurelio Colosimo's avatar Aurelio Colosimo

state-uncalibrated: handle followup and sync

parent 6abaeb90
...@@ -129,6 +129,9 @@ struct pp_instance { ...@@ -129,6 +129,9 @@ struct pp_instance {
struct pp_frgn_master *frgn_master; struct pp_frgn_master *frgn_master;
Octet *buf_out; Octet *buf_out;
Octet *buf_in; /* FIXME really useful? Probably not*/ Octet *buf_in; /* FIXME really useful? Probably not*/
TimeInternal sync_receive_time;
UInteger16 recv_sync_sequence_id;
TimeInternal last_sync_correction_field;
union { union {
MsgSync sync; MsgSync sync;
...@@ -147,8 +150,10 @@ struct pp_instance { ...@@ -147,8 +150,10 @@ struct pp_instance {
*/ */
MsgHeader msg_tmp_header; MsgHeader msg_tmp_header;
MsgHeader pdelay_req_hdr; MsgHeader pdelay_req_hdr;
Boolean is_from_self; UInteger32
Boolean is_from_cur_par; is_from_self:1,
is_from_cur_par:1,
waiting_for_follow:1;
}; };
#define DSDEF(x) x->defaultDS #define DSDEF(x) x->defaultDS
...@@ -182,6 +187,12 @@ extern int pp_timer_start(uint32_t interval, struct pp_timer *tm); ...@@ -182,6 +187,12 @@ extern int pp_timer_start(uint32_t interval, struct pp_timer *tm);
extern int pp_timer_stop(struct pp_timer *tm); extern int pp_timer_stop(struct pp_timer *tm);
extern int pp_timer_expired(struct pp_timer *tm); /* returns 1 when expired */ extern int pp_timer_expired(struct pp_timer *tm); /* returns 1 when expired */
/* Servo */
extern void pp_update_offset(TimeInternal *send_time, TimeInternal *recv_time,
TimeInternal *correctionField, struct pp_instance *ppi);
/* FIXME: offset_from_master_filter: put it in ppi */
extern void pp_update_clock(struct pp_instance *ppi);
/* bmc.c */ /* bmc.c */
extern void m1(struct pp_instance *ppi); extern void m1(struct pp_instance *ppi);
...@@ -217,6 +228,17 @@ extern void msg_pack_pdelay_resp_followup(void *buf, MsgHeader *hdr, ...@@ -217,6 +228,17 @@ extern void msg_pack_pdelay_resp_followup(void *buf, MsgHeader *hdr,
extern void msg_unpack_pdelay_resp_followup(void *buf, extern void msg_unpack_pdelay_resp_followup(void *buf,
MsgPDelayRespFollowUp *presp_follow); MsgPDelayRespFollowUp *presp_follow);
/* arith.c */
/* FIXME: add prefix in function name? */
extern void int64_to_TimeInternal(Integer64 bigint, TimeInternal *internal);
extern void from_TimeInternal(TimeInternal *internal, Timestamp *external);
extern void to_TimeInternal(TimeInternal *internal, Timestamp *external);
extern void normalize_TimeInternal(TimeInternal *r);
extern void add_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y);
extern void sub_TimeInternal(TimeInternal *r, TimeInternal *x, TimeInternal *y);
/* Get a timestamp */ /* Get a timestamp */
extern void pp_get_stamp(uint32_t *sptr); extern void pp_get_stamp(uint32_t *sptr);
......
...@@ -20,6 +20,8 @@ OBJ-libstd := $D/state-table-default.o \ ...@@ -20,6 +20,8 @@ OBJ-libstd := $D/state-table-default.o \
$D/state-common-fun.o \ $D/state-common-fun.o \
$D/bmc.o \ $D/bmc.o \
$D/msg.o \ $D/msg.o \
$D/arith.o \
$D/servo.o \
$D/open-close.o $D/open-close.o
$(TARGET).o: $(LIBSTD) $(TARGET).o: $(LIBSTD)
......
...@@ -4,6 +4,9 @@ ...@@ -4,6 +4,9 @@
#include <pproto/pproto.h> #include <pproto/pproto.h>
/* FIXME: This is a temp workaround. How to define it? */
#define PP_INT_MAX 2147483647
void int64_to_TimeInternal(Integer64 bigint, TimeInternal *internal) void int64_to_TimeInternal(Integer64 bigint, TimeInternal *internal)
{ {
int64_t bigint_val; int64_t bigint_val;
...@@ -27,8 +30,8 @@ void from_TimeInternal(TimeInternal *internal, Timestamp *external) ...@@ -27,8 +30,8 @@ void from_TimeInternal(TimeInternal *internal, Timestamp *external)
* so there is no problem here. * so there is no problem here.
*/ */
if ((internal->seconds & ~INT_MAX) || if ((internal->seconds & ~PP_INT_MAX) ||
(internal->nanoseconds & ~INT_MAX)) { (internal->nanoseconds & ~PP_INT_MAX)) {
/* FIXME diag /* FIXME diag
* DBG("Negative value cannot be converted into timestamp \n"); * DBG("Negative value cannot be converted into timestamp \n");
*/ */
...@@ -43,7 +46,7 @@ void from_TimeInternal(TimeInternal *internal, Timestamp *external) ...@@ -43,7 +46,7 @@ void from_TimeInternal(TimeInternal *internal, Timestamp *external)
void to_TimeInternal(TimeInternal *internal, Timestamp *external) void to_TimeInternal(TimeInternal *internal, Timestamp *external)
{ {
/* Program will not run after 2038... */ /* Program will not run after 2038... */
if (external->secondsField.lsb < INT_MAX) { if (external->secondsField.lsb < PP_INT_MAX) {
internal->seconds = external->secondsField.lsb; internal->seconds = external->secondsField.lsb;
internal->nanoseconds = external->nanosecondsField; internal->nanoseconds = external->nanosecondsField;
} else { } else {
......
/*
* FIXME header
*/
#include <pproto/pproto.h>
extern void pp_update_offset(TimeInternal *send_time, TimeInternal *recv_time,
TimeInternal *correctionField, struct pp_instance *ppi)
/* FIXME: offset_from_master_filter: put it in ppi */
{
/* TODO */
}
extern void pp_update_clock(struct pp_instance *ppi)
{
/* TODO */
}
...@@ -159,3 +159,110 @@ int st_com_slave_handle_announce(unsigned char *buf, int len, ...@@ -159,3 +159,110 @@ int st_com_slave_handle_announce(unsigned char *buf, int len,
return 0; return 0;
} }
int st_com_slave_handle_sync(unsigned char *buf, int len, TimeInternal *time,
struct pp_instance *ppi)
{
TimeInternal origin_tstamp;
TimeInternal correction_field;
MsgHeader *hdr = &ppi->msg_tmp_header;
if (len < PP_SYNC_LENGTH)
return -1;
if (ppi->is_from_self) {
return 0;
}
if (ppi->is_from_cur_par) {
ppi->sync_receive_time.seconds = time->seconds;
ppi->sync_receive_time.nanoseconds = time->nanoseconds;
/* FIXME diag check. Delete it?
if (ppi->rt_opts->recordFP)
fprintf(rtOpts->recordFP, "%d %llu\n",
header->sequenceId,
((time->seconds * 1000000000ULL) +
time->nanoseconds));
*/
if ((hdr->flagField[0] & 0x02) == PP_TWO_STEP_FLAG) {
ppi->waiting_for_follow = TRUE;
ppi->recv_sync_sequence_id = hdr->sequenceId;
/* Save correctionField of Sync message */
int64_to_TimeInternal(
hdr->correctionfield,
&correction_field);
ppi->last_sync_correction_field.seconds =
correction_field.seconds;
ppi->last_sync_correction_field.nanoseconds =
correction_field.nanoseconds;
} else {
msg_unpack_sync(buf, &ppi->msg_tmp.sync);
int64_to_TimeInternal(
ppi->msg_tmp_header.correctionfield,
&correction_field);
/* FIXME diag check
* timeInternal_display(&correctionfield);
*/
ppi->waiting_for_follow = FALSE;
to_TimeInternal(&origin_tstamp,
&ppi->msg_tmp.sync.originTimestamp);
pp_update_offset(&origin_tstamp,
&ppi->sync_receive_time,
&correction_field,ppi);
pp_update_clock(ppi);
}
}
return 0;
}
int st_com_slave_handle_followup(unsigned char *buf, int len,
struct pp_instance *ppi)
{
TimeInternal precise_orig_timestamp;
TimeInternal correction_field;
MsgHeader *hdr = &ppi->msg_tmp_header;
if (!ppi->is_from_cur_par) {
/* FIXME diag
DBGV("SequenceID doesn't match with "
"last Sync message \n");
*/
return 0;
}
if (!ppi->waiting_for_follow) {
/* FIXME diag
DBGV("Slave was not waiting a follow up "
"message \n");
*/
return 0;
}
if (ppi->recv_sync_sequence_id != hdr->sequenceId) {
/* FIXME diag
DBGV("Follow up message is not from current parent \n");
*/
return 0;
}
msg_unpack_follow_up(buf, &ppi->msg_tmp.follow);
ppi->waiting_for_follow = FALSE;
to_TimeInternal(&precise_orig_timestamp,
&ppi->msg_tmp.follow.preciseOriginTimestamp);
int64_to_TimeInternal(ppi->msg_tmp_header.correctionfield,
&correction_field);
add_TimeInternal(&correction_field,&correction_field,
&ppi->last_sync_correction_field);
pp_update_offset(&precise_orig_timestamp,
&ppi->sync_receive_time,
&correction_field, ppi);
pp_update_clock(ppi);
return 0;
}
...@@ -17,3 +17,6 @@ void st_com_add_foreign(unsigned char *buf, MsgHeader *header, ...@@ -17,3 +17,6 @@ void st_com_add_foreign(unsigned char *buf, MsgHeader *header,
int st_com_slave_handle_announce(unsigned char *buf, int len, int st_com_slave_handle_announce(unsigned char *buf, int len,
struct pp_instance *ppi); struct pp_instance *ppi);
int st_com_slave_handle_sync(unsigned char *buf, int len, TimeInternal *time,
struct pp_instance *ppi);
\ No newline at end of file
...@@ -7,6 +7,7 @@ ...@@ -7,6 +7,7 @@
int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen) int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
{ {
int e = 0; int e = 0;
TimeInternal time; /* TODO: handle it, see handle(...) in protocol.c */
switch (ppi->msg_tmp_header.messageType) { switch (ppi->msg_tmp_header.messageType) {
...@@ -15,11 +16,11 @@ int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -15,11 +16,11 @@ int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
break; break;
case PPM_SYNC: case PPM_SYNC:
/* TODO */ e = st_com_slave_handle_sync(pkt, plen, &time, ppi);
break; break;
case PPM_FOLLOW_UP: case PPM_FOLLOW_UP:
/* TODO */ e = st_com_slave_handle_followup(pkt, plen);
break; break;
default: default:
......
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