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#include <linux/io.h>
36#include <linux/ptp_clock_kernel.h>
37#include <linux/slab.h>
38
39#include "mvpp2.h"
40
41#define CR0_SW_NRESET BIT(0)
42
43#define TCFCR0_PHASE_UPDATE_ENABLE BIT(8)
44#define TCFCR0_TCF_MASK (7 << 2)
45#define TCFCR0_TCF_UPDATE (0 << 2)
46#define TCFCR0_TCF_FREQUPDATE (1 << 2)
47#define TCFCR0_TCF_INCREMENT (2 << 2)
48#define TCFCR0_TCF_DECREMENT (3 << 2)
49#define TCFCR0_TCF_CAPTURE (4 << 2)
50#define TCFCR0_TCF_NOP (7 << 2)
51#define TCFCR0_TCF_TRIGGER BIT(0)
52
53#define TCSR_CAPTURE_1_VALID BIT(1)
54#define TCSR_CAPTURE_0_VALID BIT(0)
55
56struct mvpp2_tai {
57 struct ptp_clock_info caps;
58 struct ptp_clock *ptp_clock;
59 void __iomem *base;
60 spinlock_t lock;
61 u64 period;
62
63 struct timespec64 stamp;
64};
65
66static void mvpp2_tai_modify(void __iomem *reg, u32 mask, u32 set)
67{
68 u32 val;
69
70 val = readl_relaxed(reg) & ~mask;
71 val |= set & mask;
72 writel(val, reg);
73}
74
75static void mvpp2_tai_write(u32 val, void __iomem *reg)
76{
77 writel_relaxed(val & 0xffff, reg);
78}
79
80static u32 mvpp2_tai_read(void __iomem *reg)
81{
82 return readl_relaxed(reg) & 0xffff;
83}
84
85static struct mvpp2_tai *ptp_to_tai(struct ptp_clock_info *ptp)
86{
87 return container_of(ptp, struct mvpp2_tai, caps);
88}
89
90static void mvpp22_tai_read_ts(struct timespec64 *ts, void __iomem *base)
91{
92 ts->tv_sec = (u64)mvpp2_tai_read(base + 0) << 32 |
93 mvpp2_tai_read(base + 4) << 16 |
94 mvpp2_tai_read(base + 8);
95
96 ts->tv_nsec = mvpp2_tai_read(base + 12) << 16 |
97 mvpp2_tai_read(base + 16);
98
99
100 readl_relaxed(base + 20);
101 readl_relaxed(base + 24);
102}
103
104static void mvpp2_tai_write_tlv(const struct timespec64 *ts, u32 frac,
105 void __iomem *base)
106{
107 mvpp2_tai_write(ts->tv_sec >> 32, base + MVPP22_TAI_TLV_SEC_HIGH);
108 mvpp2_tai_write(ts->tv_sec >> 16, base + MVPP22_TAI_TLV_SEC_MED);
109 mvpp2_tai_write(ts->tv_sec, base + MVPP22_TAI_TLV_SEC_LOW);
110 mvpp2_tai_write(ts->tv_nsec >> 16, base + MVPP22_TAI_TLV_NANO_HIGH);
111 mvpp2_tai_write(ts->tv_nsec, base + MVPP22_TAI_TLV_NANO_LOW);
112 mvpp2_tai_write(frac >> 16, base + MVPP22_TAI_TLV_FRAC_HIGH);
113 mvpp2_tai_write(frac, base + MVPP22_TAI_TLV_FRAC_LOW);
114}
115
116static void mvpp2_tai_op(u32 op, void __iomem *base)
117{
118
119
120
121 mvpp2_tai_modify(base + MVPP22_TAI_TCFCR0,
122 TCFCR0_TCF_MASK | TCFCR0_TCF_TRIGGER,
123 op | TCFCR0_TCF_TRIGGER);
124 mvpp2_tai_modify(base + MVPP22_TAI_TCFCR0, TCFCR0_TCF_MASK,
125 TCFCR0_TCF_NOP);
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
152static u64 mvpp22_calc_frac_ppm(struct mvpp2_tai *tai, long abs_scaled_ppm)
153{
154 u64 val = tai->period * abs_scaled_ppm >> 4;
155
156 return div_u64(val, (1000000 << 12) + (abs_scaled_ppm >> 4));
157}
158
159static s32 mvpp22_calc_max_adj(struct mvpp2_tai *tai)
160{
161 return 1000000;
162}
163
164static int mvpp22_tai_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
165{
166 struct mvpp2_tai *tai = ptp_to_tai(ptp);
167 unsigned long flags;
168 void __iomem *base;
169 bool neg_adj;
170 s32 frac;
171 u64 val;
172
173 neg_adj = scaled_ppm < 0;
174 if (neg_adj)
175 scaled_ppm = -scaled_ppm;
176
177 val = mvpp22_calc_frac_ppm(tai, scaled_ppm);
178
179
180 if (neg_adj) {
181
182
183
184 if (val > 0x80000000)
185 return -ERANGE;
186
187 frac = -val;
188 } else {
189 if (val > S32_MAX)
190 return -ERANGE;
191
192 frac = val;
193 }
194
195 base = tai->base;
196 spin_lock_irqsave(&tai->lock, flags);
197 mvpp2_tai_write(frac >> 16, base + MVPP22_TAI_TLV_FRAC_HIGH);
198 mvpp2_tai_write(frac, base + MVPP22_TAI_TLV_FRAC_LOW);
199 mvpp2_tai_op(TCFCR0_TCF_FREQUPDATE, base);
200 spin_unlock_irqrestore(&tai->lock, flags);
201
202 return 0;
203}
204
205static int mvpp22_tai_adjtime(struct ptp_clock_info *ptp, s64 delta)
206{
207 struct mvpp2_tai *tai = ptp_to_tai(ptp);
208 struct timespec64 ts;
209 unsigned long flags;
210 void __iomem *base;
211 u32 tcf;
212
213
214 if (delta == S64_MIN)
215 return -ERANGE;
216
217 if (delta < 0) {
218 delta = -delta;
219 tcf = TCFCR0_TCF_DECREMENT;
220 } else {
221 tcf = TCFCR0_TCF_INCREMENT;
222 }
223
224 ts = ns_to_timespec64(delta);
225
226 base = tai->base;
227 spin_lock_irqsave(&tai->lock, flags);
228 mvpp2_tai_write_tlv(&ts, 0, base);
229 mvpp2_tai_op(tcf, base);
230 spin_unlock_irqrestore(&tai->lock, flags);
231
232 return 0;
233}
234
235static int mvpp22_tai_gettimex64(struct ptp_clock_info *ptp,
236 struct timespec64 *ts,
237 struct ptp_system_timestamp *sts)
238{
239 struct mvpp2_tai *tai = ptp_to_tai(ptp);
240 unsigned long flags;
241 void __iomem *base;
242 u32 tcsr;
243 int ret;
244
245 base = tai->base;
246 spin_lock_irqsave(&tai->lock, flags);
247
248
249
250
251
252 ptp_read_system_prets(sts);
253 mvpp2_tai_modify(base + MVPP22_TAI_TCFCR0,
254 TCFCR0_TCF_MASK | TCFCR0_TCF_TRIGGER,
255 TCFCR0_TCF_CAPTURE | TCFCR0_TCF_TRIGGER);
256 ptp_read_system_postts(sts);
257 mvpp2_tai_modify(base + MVPP22_TAI_TCFCR0, TCFCR0_TCF_MASK,
258 TCFCR0_TCF_NOP);
259
260 tcsr = readl(base + MVPP22_TAI_TCSR);
261 if (tcsr & TCSR_CAPTURE_1_VALID) {
262 mvpp22_tai_read_ts(ts, base + MVPP22_TAI_TCV1_SEC_HIGH);
263 ret = 0;
264 } else if (tcsr & TCSR_CAPTURE_0_VALID) {
265 mvpp22_tai_read_ts(ts, base + MVPP22_TAI_TCV0_SEC_HIGH);
266 ret = 0;
267 } else {
268
269 ret = -EBUSY;
270 }
271 spin_unlock_irqrestore(&tai->lock, flags);
272
273 return ret;
274}
275
276static int mvpp22_tai_settime64(struct ptp_clock_info *ptp,
277 const struct timespec64 *ts)
278{
279 struct mvpp2_tai *tai = ptp_to_tai(ptp);
280 unsigned long flags;
281 void __iomem *base;
282
283 base = tai->base;
284 spin_lock_irqsave(&tai->lock, flags);
285 mvpp2_tai_write_tlv(ts, 0, base);
286
287
288
289
290
291 mvpp2_tai_modify(base + MVPP22_TAI_TCFCR0,
292 TCFCR0_PHASE_UPDATE_ENABLE |
293 TCFCR0_TCF_MASK | TCFCR0_TCF_TRIGGER,
294 TCFCR0_TCF_UPDATE | TCFCR0_TCF_TRIGGER);
295 mvpp2_tai_modify(base + MVPP22_TAI_TCFCR0, TCFCR0_TCF_MASK,
296 TCFCR0_TCF_NOP);
297 spin_unlock_irqrestore(&tai->lock, flags);
298
299 return 0;
300}
301
302static long mvpp22_tai_aux_work(struct ptp_clock_info *ptp)
303{
304 struct mvpp2_tai *tai = ptp_to_tai(ptp);
305
306 mvpp22_tai_gettimex64(ptp, &tai->stamp, NULL);
307
308 return msecs_to_jiffies(2000);
309}
310
311static void mvpp22_tai_set_step(struct mvpp2_tai *tai)
312{
313 void __iomem *base = tai->base;
314 u32 nano, frac;
315
316 nano = upper_32_bits(tai->period);
317 frac = lower_32_bits(tai->period);
318
319
320
321
322 if (frac >= 0x80000000)
323 nano += 1;
324
325 mvpp2_tai_write(nano, base + MVPP22_TAI_TOD_STEP_NANO_CR);
326 mvpp2_tai_write(frac >> 16, base + MVPP22_TAI_TOD_STEP_FRAC_HIGH);
327 mvpp2_tai_write(frac, base + MVPP22_TAI_TOD_STEP_FRAC_LOW);
328}
329
330static void mvpp22_tai_init(struct mvpp2_tai *tai)
331{
332 void __iomem *base = tai->base;
333
334 mvpp22_tai_set_step(tai);
335
336
337 mvpp2_tai_modify(base + MVPP22_TAI_CR0, CR0_SW_NRESET, CR0_SW_NRESET);
338}
339
340int mvpp22_tai_ptp_clock_index(struct mvpp2_tai *tai)
341{
342 return ptp_clock_index(tai->ptp_clock);
343}
344
345void mvpp22_tai_tstamp(struct mvpp2_tai *tai, u32 tstamp,
346 struct skb_shared_hwtstamps *hwtstamp)
347{
348 struct timespec64 ts;
349 int delta;
350
351
352
353
354
355 ts.tv_sec = READ_ONCE(tai->stamp.tv_sec);
356 ts.tv_nsec = tstamp & 0x3fffffff;
357
358
359
360
361
362 delta = ((tstamp >> 30) - (ts.tv_sec & 3)) & 3;
363 if (delta == 3)
364 delta -= 4;
365 ts.tv_sec += delta;
366
367 memset(hwtstamp, 0, sizeof(*hwtstamp));
368 hwtstamp->hwtstamp = timespec64_to_ktime(ts);
369}
370
371void mvpp22_tai_start(struct mvpp2_tai *tai)
372{
373 long delay;
374
375 delay = mvpp22_tai_aux_work(&tai->caps);
376
377 ptp_schedule_worker(tai->ptp_clock, delay);
378}
379
380void mvpp22_tai_stop(struct mvpp2_tai *tai)
381{
382 ptp_cancel_worker_sync(tai->ptp_clock);
383}
384
385static void mvpp22_tai_remove(void *priv)
386{
387 struct mvpp2_tai *tai = priv;
388
389 if (!IS_ERR(tai->ptp_clock))
390 ptp_clock_unregister(tai->ptp_clock);
391}
392
393int mvpp22_tai_probe(struct device *dev, struct mvpp2 *priv)
394{
395 struct mvpp2_tai *tai;
396 int ret;
397
398 tai = devm_kzalloc(dev, sizeof(*tai), GFP_KERNEL);
399 if (!tai)
400 return -ENOMEM;
401
402 spin_lock_init(&tai->lock);
403
404 tai->base = priv->iface_base;
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433 tai->period = 3ULL << 32;
434
435 mvpp22_tai_init(tai);
436
437 tai->caps.owner = THIS_MODULE;
438 strscpy(tai->caps.name, "Marvell PP2.2", sizeof(tai->caps.name));
439 tai->caps.max_adj = mvpp22_calc_max_adj(tai);
440 tai->caps.adjfine = mvpp22_tai_adjfine;
441 tai->caps.adjtime = mvpp22_tai_adjtime;
442 tai->caps.gettimex64 = mvpp22_tai_gettimex64;
443 tai->caps.settime64 = mvpp22_tai_settime64;
444 tai->caps.do_aux_work = mvpp22_tai_aux_work;
445
446 ret = devm_add_action(dev, mvpp22_tai_remove, tai);
447 if (ret)
448 return ret;
449
450 tai->ptp_clock = ptp_clock_register(&tai->caps, dev);
451 if (IS_ERR(tai->ptp_clock))
452 return PTR_ERR(tai->ptp_clock);
453
454 priv->tai = tai;
455
456 return 0;
457}
458