From a1bee8fbb797f184c9c23c5575e7e7f158632633 Mon Sep 17 00:00:00 2001 From: Jiri Benc Date: Wed, 11 Jun 2014 21:35:11 +0200 Subject: [PATCH] phc2sys: generalize run_pmc Make run_pmc usable for any kind of management message. Create wrappers for waiting for ptp4l and for getting UTC offset. Signed-off-by: Jiri Benc --- phc2sys.c | 135 +++++++++++++++++++++++++++--------------------------- 1 file changed, 68 insertions(+), 67 deletions(-) diff --git a/phc2sys.c b/phc2sys.c index 5ecb602..0581eb5 100644 --- a/phc2sys.c +++ b/phc2sys.c @@ -141,7 +141,6 @@ struct clock { int leap_set; int kernel_leap; struct pmc *pmc; - int pmc_ds_idx; int pmc_ds_requested; uint64_t pmc_last_update; struct clockcheck *sanity_check; @@ -390,31 +389,14 @@ static int init_pmc(struct clock *clock, int domain_number) return 0; } -static int run_pmc(struct clock *clock, int timeout, - int wait_sync, int get_utc_offset) +static int run_pmc(struct clock *clock, int timeout, int ds_id, + struct ptp_message **msg) { - struct ptp_message *msg; - struct timePropertiesDS *tds; - void *data; #define N_FD 1 struct pollfd pollfd[N_FD]; - int cnt, ds_done; -#define N_ID 2 - int ds_ids[N_ID] = { - PORT_DATA_SET, - TIME_PROPERTIES_DATA_SET - }; - - while (clock->pmc_ds_idx < N_ID) { - /* Check if the data set is really needed. */ - if ((ds_ids[clock->pmc_ds_idx] == PORT_DATA_SET && - !wait_sync) || - (ds_ids[clock->pmc_ds_idx] == TIME_PROPERTIES_DATA_SET && - !get_utc_offset)) { - clock->pmc_ds_idx++; - continue; - } + int cnt; + while (1) { pollfd[0].fd = pmc_get_transport_fd(clock->pmc); pollfd[0].events = POLLIN|POLLPRI; if (!clock->pmc_ds_requested) @@ -434,62 +416,76 @@ static int run_pmc(struct clock *clock, int timeout, /* Send a new request if there are no pending messages. */ if ((pollfd[0].revents & POLLOUT) && !(pollfd[0].revents & (POLLIN|POLLPRI))) { - pmc_send_get_action(clock->pmc, - ds_ids[clock->pmc_ds_idx]); + pmc_send_get_action(clock->pmc, ds_id); clock->pmc_ds_requested = 1; } if (!(pollfd[0].revents & (POLLIN|POLLPRI))) continue; - msg = pmc_recv(clock->pmc); + *msg = pmc_recv(clock->pmc); - if (!msg) + if (!*msg) continue; - if (!is_msg_mgt(msg) || - get_mgt_id(msg) != ds_ids[clock->pmc_ds_idx]) { - msg_put(msg); + if (!is_msg_mgt(*msg) || + get_mgt_id(*msg) != ds_id) { + msg_put(*msg); + *msg = NULL; continue; } + clock->pmc_ds_requested = 0; + return 1; + } +} + +static int run_pmc_wait_sync(struct clock *clock, int timeout) +{ + struct ptp_message *msg; + int res; + void *data; + Enumeration8 portState; + + while (1) { + res = run_pmc(clock, timeout, PORT_DATA_SET, &msg); + if (res <= 0) + return res; data = get_mgt_data(msg); - ds_done = 0; - - switch (get_mgt_id(msg)) { - case PORT_DATA_SET: - switch (((struct portDS *)data)->portState) { - case PS_MASTER: - case PS_SLAVE: - ds_done = 1; - break; - } - - break; - case TIME_PROPERTIES_DATA_SET: - tds = (struct timePropertiesDS *)data; - if (tds->flags & PTP_TIMESCALE) { - clock->sync_offset = tds->currentUtcOffset; - if (tds->flags & LEAP_61) - clock->leap = 1; - else if (tds->flags & LEAP_59) - clock->leap = -1; - else - clock->leap = 0; - } - ds_done = 1; - break; - } - - if (ds_done) { - /* Proceed with the next data set. */ - clock->pmc_ds_idx++; - clock->pmc_ds_requested = 0; - } + portState = ((struct portDS *)data)->portState; msg_put(msg); - } - clock->pmc_ds_idx = 0; + switch (portState) { + case PS_MASTER: + case PS_SLAVE: + return 1; + } + /* try to get more data sets (for other ports) */ + clock->pmc_ds_requested = 1; + } +} + +static int run_pmc_get_utc_offset(struct clock *clock, int timeout) +{ + struct ptp_message *msg; + int res; + struct timePropertiesDS *tds; + + res = run_pmc(clock, timeout, TIME_PROPERTIES_DATA_SET, &msg); + if (res <= 0) + return res; + + tds = (struct timePropertiesDS *)get_mgt_data(msg); + if (tds->flags & PTP_TIMESCALE) { + clock->sync_offset = tds->currentUtcOffset; + if (tds->flags & LEAP_61) + clock->leap = 1; + else if (tds->flags & LEAP_59) + clock->leap = -1; + else + clock->leap = 0; + } + msg_put(msg); return 1; } @@ -506,7 +502,7 @@ static int update_sync_offset(struct clock *clock, int64_t offset, uint64_t ts) if (clock->pmc && !(ts > clock->pmc_last_update && ts - clock->pmc_last_update < PMC_UPDATE_INTERVAL)) { - if (run_pmc(clock, 0, 0, 1) > 0) + if (run_pmc_get_utc_offset(clock, 0) > 0) clock->pmc_last_update = ts; } @@ -767,17 +763,22 @@ int main(int argc, char *argv[]) return -1; while (1) { - r = run_pmc(&dst_clock, 1000, - wait_sync, !forced_sync_offset); + r = run_pmc_wait_sync(&dst_clock, 1000); if (r < 0) return -1; - else if (r > 0) + if (r > 0) break; else pr_notice("Waiting for ptp4l..."); } if (!forced_sync_offset) { + r = run_pmc_get_utc_offset(&dst_clock, 1000); + if (r <= 0) { + pr_err("failed to get UTC offset"); + return -1; + } + if (src != CLOCK_REALTIME && dst_clock.clkid == CLOCK_REALTIME) dst_clock.sync_offset_direction = 1;