1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
|
// 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");
|
