diff --git a/tsf_sync_rt_starter.c b/tsf_sync_rt_starter.c index 2f0eeb5..bd809b7 100644 --- a/tsf_sync_rt_starter.c +++ b/tsf_sync_rt_starter.c @@ -219,9 +219,11 @@ static void welford_add(struct follower_acc *acc, double x) static double follower_var_pop(const struct follower_acc *a) { - if (a->n == 0) - return 0.0; - return a->m2 / (double)a->n; + double v = 0.0; + + if (a->n != 0) + v = a->m2 / (double)a->n; + return v; } static void acc_add_sample(struct follower_acc *acc, int64_t err, int64_t step, @@ -279,18 +281,21 @@ static double kf1_update(struct kf1 *k, double z, double q, double r) { double p_pred; double Kgain; + double xf; if (!k->inited) { k->x = z; k->P = r; k->inited = 1; - return k->x; + xf = k->x; + } else { + p_pred = k->P + q; + Kgain = p_pred / (p_pred + r); + k->x += Kgain * (z - k->x); + k->P = (1.0 - Kgain) * p_pred; + xf = k->x; } - p_pred = k->P + q; - Kgain = p_pred / (p_pred + r); - k->x += Kgain * (z - k->x); - k->P = (1.0 - Kgain) * p_pred; - return k->x; + return xf; } static void stats_maybe_print(const struct cli *cli, struct follower_acc *acc, @@ -299,16 +304,16 @@ static void stats_maybe_print(const struct cli *cli, struct follower_acc *acc, struct timespec now; if (!cli->stats_interval_sec || !acc) - return; + goto out; if (clock_gettime(CLOCK_MONOTONIC, &now) < 0) - return; + goto out; if (last_mono->tv_sec == 0 && last_mono->tv_nsec == 0) { *last_mono = now; - return; + goto out; } if ((unsigned long)(now.tv_sec - last_mono->tv_sec) < cli->stats_interval_sec) - return; + goto out; if (mu) pthread_mutex_lock(mu); stats_print_and_reset(stderr, cli->follower_paths, cli->n_followers, @@ -316,6 +321,8 @@ static void stats_maybe_print(const struct cli *cli, struct follower_acc *acc, if (mu) pthread_mutex_unlock(mu); *last_mono = now; +out: + return; } static void apply_correction(const struct cli *cli, uint64_t master, @@ -332,7 +339,7 @@ static void apply_correction(const struct cli *cli, uint64_t master, if (read_tsf_hex(slave_path, &slave) < 0) { fprintf(stderr, "read follower TSF failed: %s\n", slave_path); - return; + goto out; } err = (int64_t)(master - slave); if (cli->use_kalman && kf_states) @@ -355,10 +362,12 @@ static void apply_correction(const struct cli *cli, uint64_t master, if (write_tsf_decimal(slave_path, new_tsf) < 0) { fprintf(stderr, "set follower TSF failed: %s\n", slave_path); - return; + goto out; } if (stats_all) acc_add_sample(&stats_all[idx], err, step, stats_mu); +out: + return; } /* ---------- single RT thread (default) ---------- */ @@ -760,15 +769,16 @@ static int run_single(const struct cli *cli) stats = calloc(cli->n_followers, sizeof(struct follower_acc)); if (!stats) { perror("calloc stats"); - return 1; + ret = 1; + goto out; } } if (cli->use_kalman) { kf = calloc(cli->n_followers, sizeof(struct kf1)); if (!kf) { perror("calloc kf"); - free(stats); - return 1; + ret = 1; + goto out; } } sctx.stats = stats; @@ -786,6 +796,7 @@ static int run_single(const struct cli *cli) } pthread_attr_destroy(&attr); +out: free(kf); free(stats); return ret; @@ -800,21 +811,23 @@ int main(int argc, char **argv) pr = parse_cli(argc, argv, &cli); if (pr < 0) { ret = 1; - } else if (pr == 0) { - ret = 0; - } else { - if (mlockall(MCL_CURRENT | MCL_FUTURE) < 0) - perror("mlockall (ignore if unprivileged)"); - - setup_shutdown_signals(); - block_sigint_term_in_this_thread(); - - if (cli.parallel) - ret = run_parallel(&cli); - else - ret = run_single(&cli); + goto out; } + if (pr == 0) { + ret = 0; + goto out; + } + if (mlockall(MCL_CURRENT | MCL_FUTURE) < 0) + perror("mlockall (ignore if unprivileged)"); + setup_shutdown_signals(); + block_sigint_term_in_this_thread(); + + if (cli.parallel) + ret = run_parallel(&cli); + else + ret = run_single(&cli); +out: free(cli.follower_paths); return ret; }