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 {
struct pp_frgn_master *frgn_master;
Octet *buf_out;
Octet *buf_in; /* FIXME really useful? Probably not*/
TimeInternal sync_receive_time;
UInteger16 recv_sync_sequence_id;
TimeInternal last_sync_correction_field;
union {
MsgSync sync;
......@@ -147,8 +150,10 @@ struct pp_instance {
*/
MsgHeader msg_tmp_header;
MsgHeader pdelay_req_hdr;
Boolean is_from_self;
Boolean is_from_cur_par;
UInteger32
is_from_self:1,
is_from_cur_par:1,
waiting_for_follow:1;
};
#define DSDEF(x) x->defaultDS
......@@ -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_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 */
extern void m1(struct pp_instance *ppi);
......@@ -217,6 +228,17 @@ extern void msg_pack_pdelay_resp_followup(void *buf, MsgHeader *hdr,
extern void msg_unpack_pdelay_resp_followup(void *buf,
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 */
extern void pp_get_stamp(uint32_t *sptr);
......
......@@ -20,6 +20,8 @@ OBJ-libstd := $D/state-table-default.o \
$D/state-common-fun.o \
$D/bmc.o \
$D/msg.o \
$D/arith.o \
$D/servo.o \
$D/open-close.o
$(TARGET).o: $(LIBSTD)
......
......@@ -4,6 +4,9 @@
#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)
{
int64_t bigint_val;
......@@ -27,8 +30,8 @@ void from_TimeInternal(TimeInternal *internal, Timestamp *external)
* so there is no problem here.
*/
if ((internal->seconds & ~INT_MAX) ||
(internal->nanoseconds & ~INT_MAX)) {
if ((internal->seconds & ~PP_INT_MAX) ||
(internal->nanoseconds & ~PP_INT_MAX)) {
/* FIXME diag
* DBG("Negative value cannot be converted into timestamp \n");
*/
......@@ -43,7 +46,7 @@ void from_TimeInternal(TimeInternal *internal, Timestamp *external)
void to_TimeInternal(TimeInternal *internal, Timestamp *external)
{
/* 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->nanoseconds = external->nanosecondsField;
} 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,
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,
int st_com_slave_handle_announce(unsigned char *buf, int len,
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 @@
int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
{
int e = 0;
TimeInternal time; /* TODO: handle it, see handle(...) in protocol.c */
switch (ppi->msg_tmp_header.messageType) {
......@@ -15,11 +16,11 @@ int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
break;
case PPM_SYNC:
/* TODO */
e = st_com_slave_handle_sync(pkt, plen, &time, ppi);
break;
case PPM_FOLLOW_UP:
/* TODO */
e = st_com_slave_handle_followup(pkt, plen);
break;
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