From cb53238d5d1a1b9319536b4df1edf237e428d931 Mon Sep 17 00:00:00 2001 From: Hangbin Liu Date: Mon, 9 Oct 2017 22:31:48 +0800 Subject: [PATCH] phc2sys: split servo_add from function clock_add We also need this part in clock_reinit() later. So split it out of function clock_add(). Signed-off-by: Hangbin Liu --- phc2sys.c | 57 +++++++++++++++++++++++++++++++++++-------------------- 1 file changed, 36 insertions(+), 21 deletions(-) diff --git a/phc2sys.c b/phc2sys.c index e2b5c47..0472c68 100644 --- a/phc2sys.c +++ b/phc2sys.c @@ -162,12 +162,45 @@ static clockid_t clock_open(char *device, int *phc_index) return clkid; } +static struct servo *servo_add(struct node *node, struct clock *clock) +{ + double ppb; + int max_ppb; + struct servo *servo; + + clockadj_init(clock->clkid); + ppb = clockadj_get_freq(clock->clkid); + /* The reading may silently fail and return 0, reset the frequency to + make sure ppb is the actual frequency of the clock. */ + clockadj_set_freq(clock->clkid, ppb); + if (clock->clkid == CLOCK_REALTIME) { + sysclk_set_leap(0); + max_ppb = sysclk_max_freq(); + } else { + max_ppb = phc_max_adj(clock->clkid); + if (!max_ppb) { + pr_err("clock is not adjustable"); + return NULL; + } + } + + servo = servo_create(phc2sys_config, node->servo_type, + -ppb, max_ppb, 0); + if (!servo) { + pr_err("Failed to create servo"); + return NULL; + } + + servo_sync_interval(servo, node->phc_interval); + + return servo; +} + static struct clock *clock_add(struct node *node, char *device) { struct clock *c; clockid_t clkid = CLOCK_INVALID; - int max_ppb, phc_index = -1; - double ppb; + int phc_index = -1; if (device) { clkid = clock_open(device, &phc_index); @@ -211,25 +244,7 @@ static struct clock *clock_add(struct node *node, char *device) } } - clockadj_init(c->clkid); - ppb = clockadj_get_freq(c->clkid); - /* The reading may silently fail and return 0, reset the frequency to - make sure ppb is the actual frequency of the clock. */ - clockadj_set_freq(c->clkid, ppb); - if (c->clkid == CLOCK_REALTIME) { - sysclk_set_leap(0); - max_ppb = sysclk_max_freq(); - } else { - max_ppb = phc_max_adj(c->clkid); - if (!max_ppb) { - pr_err("clock is not adjustable"); - return NULL; - } - } - - c->servo = servo_create(phc2sys_config, node->servo_type, - -ppb, max_ppb, 0); - servo_sync_interval(c->servo, node->phc_interval); + c->servo = servo_add(node, c); if (clkid != CLOCK_REALTIME) c->sysoff_supported = (SYSOFF_SUPPORTED ==