[Linuxptp-devel] [PATCH v2 2/4] ptp4l: reset servo if failed to discipline clock

[Wojtek Wasko via Linuxptp-devel]

Signed-off-by: Wojciech Wasko <wwa...@nvidia.com>
---
 clock.c | 32 +++++++++++++++++++++++++-------
 1 file changed, 25 insertions(+), 7 deletions(-)

diff --git a/clock.c b/clock.c
index e9072ab..1e51dd8 100644
--- a/clock.c
+++ b/clock.c
@@ -1782,18 +1782,21 @@ static void clock_step_window(struct clock *c)
        c->step_window_counter = c->step_window;
 }
 
-static void clock_synchronize_locked(struct clock *c, double adj)
+static int clock_synchronize_locked(struct clock *c, double adj)
 {
        if (c->sanity_check) {
                clockcheck_freq(c->sanity_check, clockadj_get_freq(c->clkid));
        }
-       clockadj_set_freq(c->clkid, -adj);
+       if (clockadj_set_freq(c->clkid, -adj)) {
+               return -1;
+       }
        if (c->clkid == CLOCK_REALTIME) {
                sysclk_set_sync();
        }
        if (c->sanity_check) {
                clockcheck_set_freq(c->sanity_check, -adj);
        }
+       return 0;
 }
 
 enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t 
origin)
@@ -1845,8 +1848,12 @@ enum servo_state clock_synchronize(struct clock *c, 
tmv_t ingress, tmv_t origin)
        case SERVO_UNLOCKED:
                break;
        case SERVO_JUMP:
-               clockadj_set_freq(c->clkid, -adj);
-               clockadj_step(c->clkid, -tmv_to_nanoseconds(c->master_offset));
+               if (clockadj_set_freq(c->clkid, -adj)) {
+                       goto servo_unlock;
+               }
+               if (clockadj_step(c->clkid, 
-tmv_to_nanoseconds(c->master_offset))) {
+                       goto servo_unlock;
+               }
                c->ingress_ts = tmv_zero();
                if (c->sanity_check) {
                        clockcheck_set_freq(c->sanity_check, -adj);
@@ -1857,14 +1864,20 @@ enum servo_state clock_synchronize(struct clock *c, 
tmv_t ingress, tmv_t origin)
                clock_step_window(c);
                break;
        case SERVO_LOCKED:
-               clock_synchronize_locked(c, adj);
+               if (clock_synchronize_locked(c, adj)) {
+                       goto servo_unlock;
+               }
                break;
        case SERVO_LOCKED_STABLE:
                if (c->write_phase_mode) {
-                       clockadj_set_phase(c->clkid, -offset);
+                       if (clockadj_set_phase(c->clkid, -offset)) {
+                               goto servo_unlock;
+                       }
                        adj = 0;
                } else {
-                       clock_synchronize_locked(c, adj);
+                       if (clock_synchronize_locked(c, adj)) {
+                               goto servo_unlock;
+                       }
                }
                break;
        }
@@ -1881,6 +1894,11 @@ enum servo_state clock_synchronize(struct clock *c, 
tmv_t ingress, tmv_t origin)
        clock_notify_event(c, NOTIFY_TIME_SYNC);
 
        return state;
+
+servo_unlock:
+       servo_reset(c->servo);
+       c->servo_state = SERVO_UNLOCKED;
+       return SERVO_UNLOCKED;
 }
 
 void clock_sync_interval(struct clock *c, int n)
-- 
2.25.1


_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel