FiWiTSF/tsf_affine.c

/* SPDX-License-Identifier: GPL-2.0 OR MIT */

#include "tsf_affine.h"

#include <math.h>
#include <stdlib.h>
#include <string.h>

/* TSF is uint64 µs; add a signed correction with defined wrap at 2^64. */
static uint64_t tsf_add_s64(uint64_t base, int64_t delta)
{
	if (delta >= 0)
		return base + (uint64_t)delta;
	return base - (uint64_t)(-(unsigned long long)delta);
}

static void map_refit(struct tsf_affine_map *m)
{
	size_t n = m->win_count;
	double sum_dm = 0.0, sum_dr = 0.0;
	double var_dm = 0.0, cov_dm_dr = 0.0;
	size_t i;

	if (n == 0) {
		m->alpha = 1.0;
		m->beta = 0.0;
		return;
	}
	if (n == 1) {
		int64_t dm0 = m->dm[0];
		int64_t dr0 = m->dr[0];

		m->alpha = 1.0;
		m->beta = (double)dr0 - (double)dm0;
		return;
	}

	for (i = 0; i < n; i++) {
		sum_dm += (double)m->dm[i];
		sum_dr += (double)m->dr[i];
	}
	{
		double mean_dm = sum_dm / (double)n;
		double mean_dr = sum_dr / (double)n;

		for (i = 0; i < n; i++) {
			double x = (double)m->dm[i] - mean_dm;
			double y = (double)m->dr[i] - mean_dr;

			var_dm += x * x;
			cov_dm_dr += x * y;
		}
		if (var_dm < 1e-6) {
			m->alpha = 1.0;
			m->beta = mean_dr - mean_dm;
		} else {
			m->alpha = cov_dm_dr / var_dm;
			m->beta = mean_dr - m->alpha * mean_dm;
		}
	}
}

static int map_push_sample(struct tsf_affine_map *m, uint64_t master_tsf,
			   uint64_t radio_tsf)
{
	int64_t dm, dr;

	if (!m->have_anchor) {
		m->anchor_master = master_tsf;
		m->anchor_radio = radio_tsf;
		m->have_anchor = 1;
		m->dm[0] = 0;
		m->dr[0] = 0;
		m->win_count = 1;
		m->win_pos = 1 % m->win_cap;
		map_refit(m);
		return 0;
	}

	dm = (int64_t)(master_tsf - m->anchor_master);
	dr = (int64_t)(radio_tsf - m->anchor_radio);

	if (m->win_count < m->win_cap) {
		m->dm[m->win_count] = dm;
		m->dr[m->win_count] = dr;
		m->win_count++;
		m->win_pos = m->win_count % m->win_cap;
	} else {
		m->dm[m->win_pos] = dm;
		m->dr[m->win_pos] = dr;
		m->win_pos = (m->win_pos + 1) % m->win_cap;
	}
	map_refit(m);
	return 0;
}

static uint64_t predict_radio(const struct tsf_affine_map *m, uint64_t master_tsf)
{
	int64_t dm;
	double dr_est;

	if (!m->have_anchor)
		return master_tsf;
	dm = (int64_t)(master_tsf - m->anchor_master);
	dr_est = m->alpha * (double)dm + m->beta;
	return tsf_add_s64(m->anchor_radio, (int64_t)llround(dr_est));
}

int tsf_affine_pool_init(struct tsf_affine_pool *p, unsigned int n_radios,
			 unsigned int master_idx, size_t window_cap)
{
	unsigned int i;

	if (!p || n_radios == 0 || n_radios > TSF_AFFINE_MAX_RADIOS ||
	    master_idx >= n_radios || window_cap == 0)
		return -1;

	memset(p, 0, sizeof(*p));
	p->n_radios = n_radios;
	p->master_idx = master_idx;

	for (i = 0; i < n_radios; i++) {
		struct tsf_affine_map *m = &p->maps[i];

		m->alpha = 1.0;
		m->beta = 0.0;
		m->have_anchor = 0;
		if (i == master_idx) {
			m->win_cap = 0;
			m->dm = NULL;
			m->dr = NULL;
			continue;
		}
		m->win_cap = window_cap;
		m->dm = calloc(window_cap, sizeof(int64_t));
		m->dr = calloc(window_cap, sizeof(int64_t));
		if (!m->dm || !m->dr) {
			tsf_affine_pool_fini(p);
			return -1;
		}
	}
	return 0;
}

void tsf_affine_pool_fini(struct tsf_affine_pool *p)
{
	unsigned int i, n;

	if (!p)
		return;
	n = p->n_radios;
	for (i = 0; i < n; i++) {
		free(p->maps[i].dm);
		free(p->maps[i].dr);
		p->maps[i].dm = NULL;
		p->maps[i].dr = NULL;
	}
	memset(p, 0, sizeof(*p));
}

void tsf_affine_pool_sample(struct tsf_affine_pool *p, unsigned int radio_idx,
			    uint64_t master_tsf, uint64_t radio_tsf)
{
	if (!p || radio_idx >= p->n_radios || radio_idx == p->master_idx)
		return;
	map_push_sample(&p->maps[radio_idx], master_tsf, radio_tsf);
}

bool tsf_affine_map_ready(const struct tsf_affine_map *m)
{
	return m && m->have_anchor;
}

bool tsf_affine_pool_master_to_radio(const struct tsf_affine_pool *p,
				     unsigned int radio_idx, uint64_t master_tsf,
				     uint64_t *out_radio_tsf)
{
	if (!p || !out_radio_tsf || radio_idx >= p->n_radios)
		return false;
	if (radio_idx == p->master_idx) {
		*out_radio_tsf = master_tsf;
		return true;
	}
	if (!tsf_affine_map_ready(&p->maps[radio_idx]))
		return false;
	*out_radio_tsf = predict_radio(&p->maps[radio_idx], master_tsf);
	return true;
}

void tsf_affine_pool_master_to_all(const struct tsf_affine_pool *p,
				   uint64_t master_tsf,
				   uint64_t out_radio[TSF_AFFINE_MAX_RADIOS])
{
	unsigned int i;

	if (!p || !out_radio)
		return;
	for (i = 0; i < p->n_radios; i++) {
		if (i == p->master_idx)
			out_radio[i] = master_tsf;
		else if (tsf_affine_map_ready(&p->maps[i]))
			out_radio[i] = predict_radio(&p->maps[i], master_tsf);
		else
			out_radio[i] = 0;
	}
}