1/*
2 * TI Common Platform Time Sync
3 *
4 * Copyright (C) 2012 Richard Cochran <richardcochran@gmail.com>
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
19 */
20#include <linux/err.h>
21#include <linux/if.h>
22#include <linux/hrtimer.h>
23#include <linux/module.h>
24#include <linux/net_tstamp.h>
25#include <linux/ptp_classify.h>
26#include <linux/time.h>
27#include <linux/uaccess.h>
28#include <linux/workqueue.h>
29#include <linux/if_ether.h>
30#include <linux/if_vlan.h>
31
32#include "cpts.h"
33
34#ifdef CONFIG_TI_CPTS
35
36#define cpts_read32(c, r)	__raw_readl(&c->reg->r)
37#define cpts_write32(c, v, r)	__raw_writel(v, &c->reg->r)
38
39static int event_expired(struct cpts_event *event)
40{
41	return time_after(jiffies, event->tmo);
42}
43
44static int event_type(struct cpts_event *event)
45{
46	return (event->high >> EVENT_TYPE_SHIFT) & EVENT_TYPE_MASK;
47}
48
49static int cpts_fifo_pop(struct cpts *cpts, u32 *high, u32 *low)
50{
51	u32 r = cpts_read32(cpts, intstat_raw);
52
53	if (r & TS_PEND_RAW) {
54		*high = cpts_read32(cpts, event_high);
55		*low  = cpts_read32(cpts, event_low);
56		cpts_write32(cpts, EVENT_POP, event_pop);
57		return 0;
58	}
59	return -1;
60}
61
62/*
63 * Returns zero if matching event type was found.
64 */
65static int cpts_fifo_read(struct cpts *cpts, int match)
66{
67	int i, type = -1;
68	u32 hi, lo;
69	struct cpts_event *event;
70
71	for (i = 0; i < CPTS_FIFO_DEPTH; i++) {
72		if (cpts_fifo_pop(cpts, &hi, &lo))
73			break;
74		if (list_empty(&cpts->pool)) {
75			pr_err("cpts: event pool is empty\n");
76			return -1;
77		}
78		event = list_first_entry(&cpts->pool, struct cpts_event, list);
79		event->tmo = jiffies + 2;
80		event->high = hi;
81		event->low = lo;
82		type = event_type(event);
83		switch (type) {
84		case CPTS_EV_PUSH:
85		case CPTS_EV_RX:
86		case CPTS_EV_TX:
87			list_del_init(&event->list);
88			list_add_tail(&event->list, &cpts->events);
89			break;
90		case CPTS_EV_ROLL:
91		case CPTS_EV_HALF:
92		case CPTS_EV_HW:
93			break;
94		default:
95			pr_err("cpts: unknown event type\n");
96			break;
97		}
98		if (type == match)
99			break;
100	}
101	return type == match ? 0 : -1;
102}
103
104static cycle_t cpts_systim_read(const struct cyclecounter *cc)
105{
106	u64 val = 0;
107	struct cpts_event *event;
108	struct list_head *this, *next;
109	struct cpts *cpts = container_of(cc, struct cpts, cc);
110
111	cpts_write32(cpts, TS_PUSH, ts_push);
112	if (cpts_fifo_read(cpts, CPTS_EV_PUSH))
113		pr_err("cpts: unable to obtain a time stamp\n");
114
115	list_for_each_safe(this, next, &cpts->events) {
116		event = list_entry(this, struct cpts_event, list);
117		if (event_type(event) == CPTS_EV_PUSH) {
118			list_del_init(&event->list);
119			list_add(&event->list, &cpts->pool);
120			val = event->low;
121			break;
122		}
123	}
124
125	return val;
126}
127
128/* PTP clock operations */
129
130static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
131{
132	u64 adj;
133	u32 diff, mult;
134	int neg_adj = 0;
135	unsigned long flags;
136	struct cpts *cpts = container_of(ptp, struct cpts, info);
137
138	if (ppb < 0) {
139		neg_adj = 1;
140		ppb = -ppb;
141	}
142	mult = cpts->cc_mult;
143	adj = mult;
144	adj *= ppb;
145	diff = div_u64(adj, 1000000000ULL);
146
147	spin_lock_irqsave(&cpts->lock, flags);
148
149	timecounter_read(&cpts->tc);
150
151	cpts->cc.mult = neg_adj ? mult - diff : mult + diff;
152
153	spin_unlock_irqrestore(&cpts->lock, flags);
154
155	return 0;
156}
157
158static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
159{
160	unsigned long flags;
161	struct cpts *cpts = container_of(ptp, struct cpts, info);
162
163	spin_lock_irqsave(&cpts->lock, flags);
164	timecounter_adjtime(&cpts->tc, delta);
165	spin_unlock_irqrestore(&cpts->lock, flags);
166
167	return 0;
168}
169
170static int cpts_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
171{
172	u64 ns;
173	unsigned long flags;
174	struct cpts *cpts = container_of(ptp, struct cpts, info);
175
176	spin_lock_irqsave(&cpts->lock, flags);
177	ns = timecounter_read(&cpts->tc);
178	spin_unlock_irqrestore(&cpts->lock, flags);
179
180	*ts = ns_to_timespec64(ns);
181
182	return 0;
183}
184
185static int cpts_ptp_settime(struct ptp_clock_info *ptp,
186			    const struct timespec64 *ts)
187{
188	u64 ns;
189	unsigned long flags;
190	struct cpts *cpts = container_of(ptp, struct cpts, info);
191
192	ns = timespec64_to_ns(ts);
193
194	spin_lock_irqsave(&cpts->lock, flags);
195	timecounter_init(&cpts->tc, &cpts->cc, ns);
196	spin_unlock_irqrestore(&cpts->lock, flags);
197
198	return 0;
199}
200
201static int cpts_ptp_enable(struct ptp_clock_info *ptp,
202			   struct ptp_clock_request *rq, int on)
203{
204	return -EOPNOTSUPP;
205}
206
207static struct ptp_clock_info cpts_info = {
208	.owner		= THIS_MODULE,
209	.name		= "CTPS timer",
210	.max_adj	= 1000000,
211	.n_ext_ts	= 0,
212	.n_pins		= 0,
213	.pps		= 0,
214	.adjfreq	= cpts_ptp_adjfreq,
215	.adjtime	= cpts_ptp_adjtime,
216	.gettime64	= cpts_ptp_gettime,
217	.settime64	= cpts_ptp_settime,
218	.enable		= cpts_ptp_enable,
219};
220
221static void cpts_overflow_check(struct work_struct *work)
222{
223	struct timespec64 ts;
224	struct cpts *cpts = container_of(work, struct cpts, overflow_work.work);
225
226	cpts_write32(cpts, CPTS_EN, control);
227	cpts_write32(cpts, TS_PEND_EN, int_enable);
228	cpts_ptp_gettime(&cpts->info, &ts);
229	pr_debug("cpts overflow check at %lld.%09lu\n", ts.tv_sec, ts.tv_nsec);
230	schedule_delayed_work(&cpts->overflow_work, CPTS_OVERFLOW_PERIOD);
231}
232
233static void cpts_clk_init(struct device *dev, struct cpts *cpts)
234{
235	cpts->refclk = devm_clk_get(dev, "cpts");
236	if (IS_ERR(cpts->refclk)) {
237		dev_err(dev, "Failed to get cpts refclk\n");
238		cpts->refclk = NULL;
239		return;
240	}
241	clk_prepare_enable(cpts->refclk);
242}
243
244static void cpts_clk_release(struct cpts *cpts)
245{
246	clk_disable(cpts->refclk);
247}
248
249static int cpts_match(struct sk_buff *skb, unsigned int ptp_class,
250		      u16 ts_seqid, u8 ts_msgtype)
251{
252	u16 *seqid;
253	unsigned int offset = 0;
254	u8 *msgtype, *data = skb->data;
255
256	if (ptp_class & PTP_CLASS_VLAN)
257		offset += VLAN_HLEN;
258
259	switch (ptp_class & PTP_CLASS_PMASK) {
260	case PTP_CLASS_IPV4:
261		offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
262		break;
263	case PTP_CLASS_IPV6:
264		offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
265		break;
266	case PTP_CLASS_L2:
267		offset += ETH_HLEN;
268		break;
269	default:
270		return 0;
271	}
272
273	if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
274		return 0;
275
276	if (unlikely(ptp_class & PTP_CLASS_V1))
277		msgtype = data + offset + OFF_PTP_CONTROL;
278	else
279		msgtype = data + offset;
280
281	seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
282
283	return (ts_msgtype == (*msgtype & 0xf) && ts_seqid == ntohs(*seqid));
284}
285
286static u64 cpts_find_ts(struct cpts *cpts, struct sk_buff *skb, int ev_type)
287{
288	u64 ns = 0;
289	struct cpts_event *event;
290	struct list_head *this, *next;
291	unsigned int class = ptp_classify_raw(skb);
292	unsigned long flags;
293	u16 seqid;
294	u8 mtype;
295
296	if (class == PTP_CLASS_NONE)
297		return 0;
298
299	spin_lock_irqsave(&cpts->lock, flags);
300	cpts_fifo_read(cpts, CPTS_EV_PUSH);
301	list_for_each_safe(this, next, &cpts->events) {
302		event = list_entry(this, struct cpts_event, list);
303		if (event_expired(event)) {
304			list_del_init(&event->list);
305			list_add(&event->list, &cpts->pool);
306			continue;
307		}
308		mtype = (event->high >> MESSAGE_TYPE_SHIFT) & MESSAGE_TYPE_MASK;
309		seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK;
310		if (ev_type == event_type(event) &&
311		    cpts_match(skb, class, seqid, mtype)) {
312			ns = timecounter_cyc2time(&cpts->tc, event->low);
313			list_del_init(&event->list);
314			list_add(&event->list, &cpts->pool);
315			break;
316		}
317	}
318	spin_unlock_irqrestore(&cpts->lock, flags);
319
320	return ns;
321}
322
323void cpts_rx_timestamp(struct cpts *cpts, struct sk_buff *skb)
324{
325	u64 ns;
326	struct skb_shared_hwtstamps *ssh;
327
328	if (!cpts->rx_enable)
329		return;
330	ns = cpts_find_ts(cpts, skb, CPTS_EV_RX);
331	if (!ns)
332		return;
333	ssh = skb_hwtstamps(skb);
334	memset(ssh, 0, sizeof(*ssh));
335	ssh->hwtstamp = ns_to_ktime(ns);
336}
337
338void cpts_tx_timestamp(struct cpts *cpts, struct sk_buff *skb)
339{
340	u64 ns;
341	struct skb_shared_hwtstamps ssh;
342
343	if (!(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS))
344		return;
345	ns = cpts_find_ts(cpts, skb, CPTS_EV_TX);
346	if (!ns)
347		return;
348	memset(&ssh, 0, sizeof(ssh));
349	ssh.hwtstamp = ns_to_ktime(ns);
350	skb_tstamp_tx(skb, &ssh);
351}
352
353#endif /*CONFIG_TI_CPTS*/
354
355int cpts_register(struct device *dev, struct cpts *cpts,
356		  u32 mult, u32 shift)
357{
358#ifdef CONFIG_TI_CPTS
359	int err, i;
360	unsigned long flags;
361
362	cpts->info = cpts_info;
363	cpts->clock = ptp_clock_register(&cpts->info, dev);
364	if (IS_ERR(cpts->clock)) {
365		err = PTR_ERR(cpts->clock);
366		cpts->clock = NULL;
367		return err;
368	}
369	spin_lock_init(&cpts->lock);
370
371	cpts->cc.read = cpts_systim_read;
372	cpts->cc.mask = CLOCKSOURCE_MASK(32);
373	cpts->cc_mult = mult;
374	cpts->cc.mult = mult;
375	cpts->cc.shift = shift;
376
377	INIT_LIST_HEAD(&cpts->events);
378	INIT_LIST_HEAD(&cpts->pool);
379	for (i = 0; i < CPTS_MAX_EVENTS; i++)
380		list_add(&cpts->pool_data[i].list, &cpts->pool);
381
382	cpts_clk_init(dev, cpts);
383	cpts_write32(cpts, CPTS_EN, control);
384	cpts_write32(cpts, TS_PEND_EN, int_enable);
385
386	spin_lock_irqsave(&cpts->lock, flags);
387	timecounter_init(&cpts->tc, &cpts->cc, ktime_to_ns(ktime_get_real()));
388	spin_unlock_irqrestore(&cpts->lock, flags);
389
390	INIT_DELAYED_WORK(&cpts->overflow_work, cpts_overflow_check);
391	schedule_delayed_work(&cpts->overflow_work, CPTS_OVERFLOW_PERIOD);
392
393	cpts->phc_index = ptp_clock_index(cpts->clock);
394#endif
395	return 0;
396}
397
398void cpts_unregister(struct cpts *cpts)
399{
400#ifdef CONFIG_TI_CPTS
401	if (cpts->clock) {
402		ptp_clock_unregister(cpts->clock);
403		cancel_delayed_work_sync(&cpts->overflow_work);
404	}
405	if (cpts->refclk)
406		cpts_clk_release(cpts);
407#endif
408}
409