/**
* Function for reporting the completion of sending of the PTP event
* frame. Called for Delay_req and Sync frames (if two step clock).
* @see send.
* @param port_num port number.
* @param msg_hdr Header of the sent frame.
* @param error sending process failed, ptp error number returned.
* @param sent_time timestamp of the sent ptp frame.
*/
void ptp_frame_sent(int port_num,
struct ptp_header *msg_hdr,
int error, struct Timestamp *sent_time)
{
struct ptp_port_ctx *ctx = ptp_ctx.ports_list_head;
while (ctx != NULL) {
if (ctx->port_dataset.port_identity.port_number == port_num) {
// match found
break;
}
ctx = ctx->next;
}
if (!ctx) {
ERROR("Port not found\n");
return;
}
switch (msg_hdr->msg_type & 0x0f) {
case PTP_SYNC:
if (ptp_cfg.one_step_clock == 0) {
// This is executed only if TWO_STEP_CLOCK==1
char tmpbuf[MAX_PTP_FRAME_SIZE];
int ret = 0;
DEBUG("Sync sent, send follow_up\n");
// create follow_up
ret = create_follow_up(ctx, tmpbuf, sent_time,
htons(msg_hdr->seq_id));
if (ret > 0) {
DEBUG("Send Follow up\n");
ret = ptp_send(&ptp_ctx.pkt_ctx, PTP_FOLLOW_UP,
port_num, tmpbuf, ret);
if( ret != PTP_ERR_OK ){
socket_restart = 1;
}
}
}
break;
case PTP_DELAY_REQ:
DEBUG("delay_req sent, store timestamp\n");
// Store timestamp and seq_id
ctx->delay_req_seqid_sent = ntohs(msg_hdr->seq_id);
copy_timestamp(&ctx->delay_req_send_time, sent_time);
break;
default:
// Nothing
break;
}
}
int main(int argc, char *argv[])
{
unsigned long sleep_time;
double read_interval;
start_time = time(0);
memset(&rtiming, 0, sizeof(rtiming));
rtiming.rt_variance.v_min = FLT_MAX;
/*
* Early initialization before reading config
*/
conf_init_pre();
parse_args_pre(argc, argv);
/*
* Reading the configuration file */
configfile_read();
/*
* Late initialization after reading config
*/
if (parse_args_post(argc, argv))
return 1;
conf_init_post();
module_init();
read_interval = cfg_read_interval;
sleep_time = cfg_getint(cfg, "sleep_time");
if (((double) sleep_time / 1000000.0f) > read_interval)
sleep_time = (unsigned long) (read_interval * 1000000.0f);
DBG("Entering mainloop...");
do {
/*
* E := Elapsed time
* NR := Next Read
* LR := Last Read
* RI := Read Interval
* ST := Sleep Time
* C := Correction
*/
timestamp_t e, ri, tmp;
unsigned long st;
float_to_timestamp(&ri, read_interval);
/*
* NR := NOW
*/
update_timestamp(&rtiming.rt_next_read);
for (;;) {
output_pre();
/*
* E := NOW
*/
update_timestamp(&e);
/*
* IF NR <= E THEN
*/
if (timestamp_le(&rtiming.rt_next_read, &e)) {
timestamp_t c;
/*
* C := (NR - E)
*/
timestamp_sub(&c, &rtiming.rt_next_read, &e);
//calc_variance(&c, &ri);
/*
* LR := E
*/
copy_timestamp(&rtiming.rt_last_read, &e);
/*
* NR := E + RI + C
*/
timestamp_add(&rtiming.rt_next_read, &e, &ri);
timestamp_add(&rtiming.rt_next_read,
&rtiming.rt_next_read, &c);
reset_update_flags();
input_read();
free_unused_elements();
output_draw();
output_post();
}
if (do_quit)
exit(0);
/*
* ST := Configured ST
*/
st = sleep_time;
/*
* IF (NR - E) < ST THEN
*/
timestamp_sub(&tmp, &rtiming.rt_next_read, &e);
if (tmp.tv_sec < 0)
continue;
if (tmp.tv_sec == 0 && tmp.tv_usec < st) {
if (tmp.tv_usec < 0)
continue;
/*
* ST := (NR - E)
*/
st = tmp.tv_usec;
}
/*
* SLEEP(ST)
*/
usleep(st);
}
} while (0);
return 0; /* buddha says i'll never be reached */
}
int main(int argc, char *argv[])
{
unsigned long sleep_time;
double read_interval;
start_time = time(0);
parse_args_pre(argc, argv);
configfile_read();
parse_args_post(argc, argv);
conf_init();
module_init();
read_interval = cfg_read_interval;
sleep_time = cfg_getint(cfg, "sleep_time");
if (((double) sleep_time / 1000000.0f) > read_interval)
sleep_time = (unsigned long) (read_interval * 1000000.0f);
// pipe_start();
if (cfg_getbool(cfg, "daemon")) {
init_syslog();
daemonize();
write_pidfile();
}
drop_privs();
do {
/*
* E := Elapsed time
* NR := Next Read
* LR := Last Read
* RI := Read Interval
* ST := Sleep Time
* C := Correction
*/
timestamp_t e, ri, tmp;
unsigned long st;
float_to_timestamp(&ri, read_interval);
/*
* NR := NOW
*/
update_timestamp(&rtiming.rt_next_read);
for (;;) {
output_pre();
/*
* read the chucka chucka pipe
*/
// pipe_handle();
/*
* E := NOW
*/
update_timestamp(&e);
/*
* IF NR <= E THEN
*/
if (timestamp_le(&rtiming.rt_next_read, &e)) {
timestamp_t c;
/*
* C := (NR - E)
*/
timestamp_sub(&c, &rtiming.rt_next_read, &e);
//calc_variance(&c, &ri);
/*
* LR := E
*/
copy_timestamp(&rtiming.rt_last_read, &e);
/*
* NR := E + RI + C
*/
timestamp_add(&rtiming.rt_next_read, &e, &ri);
timestamp_add(&rtiming.rt_next_read,
&rtiming.rt_next_read, &c);
reset_update_flags();
input_read();
free_unused_elements();
output_draw();
output_post();
}
if (do_quit)
exit(0);
/*
* ST := Configured ST
*/
st = sleep_time;
/*
* IF (NR - E) < ST THEN
*/
timestamp_sub(&tmp, &rtiming.rt_next_read, &e);
if (tmp.tv_sec < 0)
continue;
if (tmp.tv_sec == 0 && tmp.tv_usec < st) {
if (tmp.tv_usec < 0)
continue;
/*
* ST := (NR - E)
*/
st = tmp.tv_usec;
}
/*
* SLEEP(ST)
*/
usleep(st);
}
} while (0);
return 0; /* buddha says i'll never be reached */
}
