original_kernel/drivers/ptp/ptp_mock.c

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * Copyright 2023 NXP
 *
 * Mock-up PTP Hardware Clock driver for virtual network devices
 *
 * Create a PTP clock which offers PTP time manipulation operations
 * using a timecounter/cyclecounter on top of CLOCK_MONOTONIC_RAW.
 */

#include <linux/ptp_clock_kernel.h>
#include <linux/ptp_mock.h>
#include <linux/timecounter.h>

/* Clamp scaled_ppm between -2,097,152,000 and 2,097,152,000,
 * and thus "adj" between -68,719,476 and 68,719,476
 */
#define MOCK_PHC_MAX_ADJ_PPB		32000000
/* Timestamps from ktime_get_raw() have 1 ns resolution, so the scale factor
 * (MULT >> SHIFT) needs to be 1. Pick SHIFT as 31 bits, which translates
 * MULT(freq 0) into 0x80000000.
 */
#define MOCK_PHC_CC_SHIFT		31
#define MOCK_PHC_CC_MULT		(1 << MOCK_PHC_CC_SHIFT)
#define MOCK_PHC_FADJ_SHIFT		9
#define MOCK_PHC_FADJ_DENOMINATOR	15625ULL

/* The largest cycle_delta that timecounter_read_delta() can handle without a
 * 64-bit overflow during the multiplication with cc->mult, given the max "adj"
 * we permit, is ~8.3 seconds. Make sure readouts are more frequent than that.
 */
#define MOCK_PHC_REFRESH_INTERVAL	(HZ * 5)

#define info_to_phc(d) container_of((d), struct mock_phc, info)

struct mock_phc {
	struct ptp_clock_info info;
	struct ptp_clock *clock;
	struct timecounter tc;
	struct cyclecounter cc;
	spinlock_t lock;
};

static u64 mock_phc_cc_read(const struct cyclecounter *cc)
{
	return ktime_get_raw_ns();
}

static int mock_phc_adjfine(struct ptp_clock_info *info, long scaled_ppm)
{
	struct mock_phc *phc = info_to_phc(info);
	s64 adj;

	adj = (s64)scaled_ppm << MOCK_PHC_FADJ_SHIFT;
	adj = div_s64(adj, MOCK_PHC_FADJ_DENOMINATOR);

	spin_lock(&phc->lock);
	timecounter_read(&phc->tc);
	phc->cc.mult = MOCK_PHC_CC_MULT + adj;
	spin_unlock(&phc->lock);

	return 0;
}

static int mock_phc_adjtime(struct ptp_clock_info *info, s64 delta)
{
	struct mock_phc *phc = info_to_phc(info);

	spin_lock(&phc->lock);
	timecounter_adjtime(&phc->tc, delta);
	spin_unlock(&phc->lock);

	return 0;
}

static int mock_phc_settime64(struct ptp_clock_info *info,
			      const struct timespec64 *ts)
{
	struct mock_phc *phc = info_to_phc(info);
	u64 ns = timespec64_to_ns(ts);

	spin_lock(&phc->lock);
	timecounter_init(&phc->tc, &phc->cc, ns);
	spin_unlock(&phc->lock);

	return 0;
}

static int mock_phc_gettime64(struct ptp_clock_info *info, struct timespec64 *ts)
{
	struct mock_phc *phc = info_to_phc(info);
	u64 ns;

	spin_lock(&phc->lock);
	ns = timecounter_read(&phc->tc);
	spin_unlock(&phc->lock);

	*ts = ns_to_timespec64(ns);

	return 0;
}

static long mock_phc_refresh(struct ptp_clock_info *info)
{
	struct timespec64 ts;

	mock_phc_gettime64(info, &ts);

	return MOCK_PHC_REFRESH_INTERVAL;
}

int mock_phc_index(struct mock_phc *phc)
{
	return ptp_clock_index(phc->clock);
}
EXPORT_SYMBOL_GPL(mock_phc_index);

struct mock_phc *mock_phc_create(struct device *dev)
{
	struct mock_phc *phc;
	int err;

	phc = kzalloc(sizeof(*phc), GFP_KERNEL);
	if (!phc) {
		err = -ENOMEM;
		goto out;
	}

	phc->info = (struct ptp_clock_info) {
		.owner		= THIS_MODULE,
		.name		= "Mock-up PTP clock",
		.max_adj	= MOCK_PHC_MAX_ADJ_PPB,
		.adjfine	= mock_phc_adjfine,
		.adjtime	= mock_phc_adjtime,
		.gettime64	= mock_phc_gettime64,
		.settime64	= mock_phc_settime64,
		.do_aux_work	= mock_phc_refresh,
	};

	phc->cc = (struct cyclecounter) {
		.read	= mock_phc_cc_read,
		.mask	= CYCLECOUNTER_MASK(64),
		.mult	= MOCK_PHC_CC_MULT,
		.shift	= MOCK_PHC_CC_SHIFT,
	};

	spin_lock_init(&phc->lock);
	timecounter_init(&phc->tc, &phc->cc, 0);

	phc->clock = ptp_clock_register(&phc->info, dev);
	if (IS_ERR(phc->clock)) {
		err = PTR_ERR(phc->clock);
		goto out_free_phc;
	}

	ptp_schedule_worker(phc->clock, MOCK_PHC_REFRESH_INTERVAL);

	return phc;

out_free_phc:
	kfree(phc);
out:
	return ERR_PTR(err);
}
EXPORT_SYMBOL_GPL(mock_phc_create);

void mock_phc_destroy(struct mock_phc *phc)
{
	ptp_clock_unregister(phc->clock);
	kfree(phc);
}
EXPORT_SYMBOL_GPL(mock_phc_destroy);

MODULE_DESCRIPTION("Mock-up PTP Hardware Clock driver");
MODULE_LICENSE("GPL");