xref: /openbmc/linux/drivers/net/ethernet/ti/cpts.c (revision c8f8e47efe66dae775b617982e47a4564d7c4dda)
168cf027fSGrygorii Strashko // SPDX-License-Identifier: GPL-2.0+
287c0e764SRichard Cochran /*
387c0e764SRichard Cochran  * TI Common Platform Time Sync
487c0e764SRichard Cochran  *
587c0e764SRichard Cochran  * Copyright (C) 2012 Richard Cochran <richardcochran@gmail.com>
687c0e764SRichard Cochran  *
787c0e764SRichard Cochran  */
8a3047a81SGrygorii Strashko #include <linux/clk-provider.h>
987c0e764SRichard Cochran #include <linux/err.h>
1087c0e764SRichard Cochran #include <linux/if.h>
1187c0e764SRichard Cochran #include <linux/hrtimer.h>
1287c0e764SRichard Cochran #include <linux/module.h>
1387c0e764SRichard Cochran #include <linux/net_tstamp.h>
1487c0e764SRichard Cochran #include <linux/ptp_classify.h>
1587c0e764SRichard Cochran #include <linux/time.h>
1687c0e764SRichard Cochran #include <linux/uaccess.h>
1787c0e764SRichard Cochran #include <linux/workqueue.h>
1879eb9d28SAlexei Starovoitov #include <linux/if_ether.h>
1979eb9d28SAlexei Starovoitov #include <linux/if_vlan.h>
2087c0e764SRichard Cochran 
2187c0e764SRichard Cochran #include "cpts.h"
2287c0e764SRichard Cochran 
230d5f54feSGrygorii Strashko #define CPTS_SKB_TX_WORK_TIMEOUT 1 /* jiffies */
24*c8f8e47eSGrygorii Strashko #define CPTS_SKB_RX_TX_TMO 100 /*ms */
25*c8f8e47eSGrygorii Strashko #define CPTS_EVENT_RX_TX_TIMEOUT (100) /* ms */
260d5f54feSGrygorii Strashko 
270d5f54feSGrygorii Strashko struct cpts_skb_cb_data {
283bfd41b5SGrygorii Strashko 	u32 skb_mtype_seqid;
290d5f54feSGrygorii Strashko 	unsigned long tmo;
300d5f54feSGrygorii Strashko };
310d5f54feSGrygorii Strashko 
32391fd6caSGrygorii Strashko #define cpts_read32(c, r)	readl_relaxed(&c->reg->r)
33391fd6caSGrygorii Strashko #define cpts_write32(c, v, r)	writel_relaxed(v, &c->reg->r)
3487c0e764SRichard Cochran 
3587c0e764SRichard Cochran static int event_expired(struct cpts_event *event)
3687c0e764SRichard Cochran {
3787c0e764SRichard Cochran 	return time_after(jiffies, event->tmo);
3887c0e764SRichard Cochran }
3987c0e764SRichard Cochran 
4087c0e764SRichard Cochran static int event_type(struct cpts_event *event)
4187c0e764SRichard Cochran {
4287c0e764SRichard Cochran 	return (event->high >> EVENT_TYPE_SHIFT) & EVENT_TYPE_MASK;
4387c0e764SRichard Cochran }
4487c0e764SRichard Cochran 
4587c0e764SRichard Cochran static int cpts_fifo_pop(struct cpts *cpts, u32 *high, u32 *low)
4687c0e764SRichard Cochran {
4787c0e764SRichard Cochran 	u32 r = cpts_read32(cpts, intstat_raw);
4887c0e764SRichard Cochran 
4987c0e764SRichard Cochran 	if (r & TS_PEND_RAW) {
5087c0e764SRichard Cochran 		*high = cpts_read32(cpts, event_high);
5187c0e764SRichard Cochran 		*low  = cpts_read32(cpts, event_low);
5287c0e764SRichard Cochran 		cpts_write32(cpts, EVENT_POP, event_pop);
5387c0e764SRichard Cochran 		return 0;
5487c0e764SRichard Cochran 	}
5587c0e764SRichard Cochran 	return -1;
5687c0e764SRichard Cochran }
5787c0e764SRichard Cochran 
58e4439fa8SWingMan Kwok static int cpts_purge_events(struct cpts *cpts)
59e4439fa8SWingMan Kwok {
60e4439fa8SWingMan Kwok 	struct list_head *this, *next;
61e4439fa8SWingMan Kwok 	struct cpts_event *event;
62e4439fa8SWingMan Kwok 	int removed = 0;
63e4439fa8SWingMan Kwok 
64e4439fa8SWingMan Kwok 	list_for_each_safe(this, next, &cpts->events) {
65e4439fa8SWingMan Kwok 		event = list_entry(this, struct cpts_event, list);
66e4439fa8SWingMan Kwok 		if (event_expired(event)) {
67e4439fa8SWingMan Kwok 			list_del_init(&event->list);
68e4439fa8SWingMan Kwok 			list_add(&event->list, &cpts->pool);
69e4439fa8SWingMan Kwok 			++removed;
70e4439fa8SWingMan Kwok 		}
71e4439fa8SWingMan Kwok 	}
72e4439fa8SWingMan Kwok 
73e4439fa8SWingMan Kwok 	if (removed)
7479d6e755SGrygorii Strashko 		dev_dbg(cpts->dev, "cpts: event pool cleaned up %d\n", removed);
75e4439fa8SWingMan Kwok 	return removed ? 0 : -1;
76e4439fa8SWingMan Kwok }
77e4439fa8SWingMan Kwok 
78f19dcd5fSIvan Khoronzhuk static void cpts_purge_txq(struct cpts *cpts)
79f19dcd5fSIvan Khoronzhuk {
80f19dcd5fSIvan Khoronzhuk 	struct cpts_skb_cb_data *skb_cb;
81f19dcd5fSIvan Khoronzhuk 	struct sk_buff *skb, *tmp;
82f19dcd5fSIvan Khoronzhuk 	int removed = 0;
83f19dcd5fSIvan Khoronzhuk 
84f19dcd5fSIvan Khoronzhuk 	skb_queue_walk_safe(&cpts->txq, skb, tmp) {
85f19dcd5fSIvan Khoronzhuk 		skb_cb = (struct cpts_skb_cb_data *)skb->cb;
86f19dcd5fSIvan Khoronzhuk 		if (time_after(jiffies, skb_cb->tmo)) {
87f19dcd5fSIvan Khoronzhuk 			__skb_unlink(skb, &cpts->txq);
88f19dcd5fSIvan Khoronzhuk 			dev_consume_skb_any(skb);
89f19dcd5fSIvan Khoronzhuk 			++removed;
90f19dcd5fSIvan Khoronzhuk 		}
91f19dcd5fSIvan Khoronzhuk 	}
92f19dcd5fSIvan Khoronzhuk 
93f19dcd5fSIvan Khoronzhuk 	if (removed)
94f19dcd5fSIvan Khoronzhuk 		dev_dbg(cpts->dev, "txq cleaned up %d\n", removed);
95f19dcd5fSIvan Khoronzhuk }
96f19dcd5fSIvan Khoronzhuk 
9787c0e764SRichard Cochran /*
9887c0e764SRichard Cochran  * Returns zero if matching event type was found.
9987c0e764SRichard Cochran  */
10087c0e764SRichard Cochran static int cpts_fifo_read(struct cpts *cpts, int match)
10187c0e764SRichard Cochran {
10287c0e764SRichard Cochran 	int i, type = -1;
10387c0e764SRichard Cochran 	u32 hi, lo;
10487c0e764SRichard Cochran 	struct cpts_event *event;
10587c0e764SRichard Cochran 
10687c0e764SRichard Cochran 	for (i = 0; i < CPTS_FIFO_DEPTH; i++) {
10787c0e764SRichard Cochran 		if (cpts_fifo_pop(cpts, &hi, &lo))
10887c0e764SRichard Cochran 			break;
109e4439fa8SWingMan Kwok 
110e4439fa8SWingMan Kwok 		if (list_empty(&cpts->pool) && cpts_purge_events(cpts)) {
11179d6e755SGrygorii Strashko 			dev_warn(cpts->dev, "cpts: event pool empty\n");
11287c0e764SRichard Cochran 			return -1;
11387c0e764SRichard Cochran 		}
114e4439fa8SWingMan Kwok 
11587c0e764SRichard Cochran 		event = list_first_entry(&cpts->pool, struct cpts_event, list);
11687c0e764SRichard Cochran 		event->high = hi;
11787c0e764SRichard Cochran 		event->low = lo;
118e66dccceSGrygorii Strashko 		event->timestamp = timecounter_cyc2time(&cpts->tc, event->low);
11987c0e764SRichard Cochran 		type = event_type(event);
120e66dccceSGrygorii Strashko 
121e66dccceSGrygorii Strashko 		dev_dbg(cpts->dev, "CPTS_EV: %d high:%08X low:%08x\n",
122e66dccceSGrygorii Strashko 			type, event->high, event->low);
12387c0e764SRichard Cochran 		switch (type) {
124e66dccceSGrygorii Strashko 		case CPTS_EV_PUSH:
125e66dccceSGrygorii Strashko 			WRITE_ONCE(cpts->cur_timestamp, lo);
126e66dccceSGrygorii Strashko 			timecounter_read(&cpts->tc);
1270d6df3e6SGrygorii Strashko 			if (cpts->mult_new) {
1280d6df3e6SGrygorii Strashko 				cpts->cc.mult = cpts->mult_new;
1290d6df3e6SGrygorii Strashko 				cpts->mult_new = 0;
1300d6df3e6SGrygorii Strashko 			}
131e66dccceSGrygorii Strashko 			break;
1320d5f54feSGrygorii Strashko 		case CPTS_EV_TX:
13387c0e764SRichard Cochran 		case CPTS_EV_RX:
134*c8f8e47eSGrygorii Strashko 			event->tmo = jiffies +
135*c8f8e47eSGrygorii Strashko 				msecs_to_jiffies(CPTS_EVENT_RX_TX_TIMEOUT);
136*c8f8e47eSGrygorii Strashko 
13787c0e764SRichard Cochran 			list_del_init(&event->list);
13887c0e764SRichard Cochran 			list_add_tail(&event->list, &cpts->events);
13987c0e764SRichard Cochran 			break;
14087c0e764SRichard Cochran 		case CPTS_EV_ROLL:
14187c0e764SRichard Cochran 		case CPTS_EV_HALF:
14287c0e764SRichard Cochran 		case CPTS_EV_HW:
14387c0e764SRichard Cochran 			break;
14487c0e764SRichard Cochran 		default:
14579d6e755SGrygorii Strashko 			dev_err(cpts->dev, "cpts: unknown event type\n");
14687c0e764SRichard Cochran 			break;
14787c0e764SRichard Cochran 		}
14887c0e764SRichard Cochran 		if (type == match)
14987c0e764SRichard Cochran 			break;
15087c0e764SRichard Cochran 	}
15187c0e764SRichard Cochran 	return type == match ? 0 : -1;
15287c0e764SRichard Cochran }
15387c0e764SRichard Cochran 
154a5a1d1c2SThomas Gleixner static u64 cpts_systim_read(const struct cyclecounter *cc)
15587c0e764SRichard Cochran {
15687c0e764SRichard Cochran 	struct cpts *cpts = container_of(cc, struct cpts, cc);
15787c0e764SRichard Cochran 
158e66dccceSGrygorii Strashko 	return READ_ONCE(cpts->cur_timestamp);
159e66dccceSGrygorii Strashko }
160e66dccceSGrygorii Strashko 
161856e59abSGrygorii Strashko static void cpts_update_cur_time(struct cpts *cpts, int match,
162856e59abSGrygorii Strashko 				 struct ptp_system_timestamp *sts)
163e66dccceSGrygorii Strashko {
164856e59abSGrygorii Strashko 	ptp_read_system_prets(sts);
16587c0e764SRichard Cochran 	cpts_write32(cpts, TS_PUSH, ts_push);
166856e59abSGrygorii Strashko 	cpts_read32(cpts, ts_push);
167856e59abSGrygorii Strashko 	ptp_read_system_postts(sts);
168e66dccceSGrygorii Strashko 
169e66dccceSGrygorii Strashko 	if (cpts_fifo_read(cpts, match) && match != -1)
17079d6e755SGrygorii Strashko 		dev_err(cpts->dev, "cpts: unable to obtain a time stamp\n");
17187c0e764SRichard Cochran }
17287c0e764SRichard Cochran 
17387c0e764SRichard Cochran /* PTP clock operations */
17487c0e764SRichard Cochran 
17587c0e764SRichard Cochran static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
17687c0e764SRichard Cochran {
17787c0e764SRichard Cochran 	u64 adj;
17887c0e764SRichard Cochran 	u32 diff, mult;
17987c0e764SRichard Cochran 	int neg_adj = 0;
18087c0e764SRichard Cochran 	unsigned long flags;
18187c0e764SRichard Cochran 	struct cpts *cpts = container_of(ptp, struct cpts, info);
18287c0e764SRichard Cochran 
18387c0e764SRichard Cochran 	if (ppb < 0) {
18487c0e764SRichard Cochran 		neg_adj = 1;
18587c0e764SRichard Cochran 		ppb = -ppb;
18687c0e764SRichard Cochran 	}
18787c0e764SRichard Cochran 	mult = cpts->cc_mult;
18887c0e764SRichard Cochran 	adj = mult;
18987c0e764SRichard Cochran 	adj *= ppb;
19087c0e764SRichard Cochran 	diff = div_u64(adj, 1000000000ULL);
19187c0e764SRichard Cochran 
19287c0e764SRichard Cochran 	spin_lock_irqsave(&cpts->lock, flags);
19387c0e764SRichard Cochran 
1940d6df3e6SGrygorii Strashko 	cpts->mult_new = neg_adj ? mult - diff : mult + diff;
19587c0e764SRichard Cochran 
196856e59abSGrygorii Strashko 	cpts_update_cur_time(cpts, CPTS_EV_PUSH, NULL);
19787c0e764SRichard Cochran 
19887c0e764SRichard Cochran 	spin_unlock_irqrestore(&cpts->lock, flags);
19987c0e764SRichard Cochran 
20087c0e764SRichard Cochran 	return 0;
20187c0e764SRichard Cochran }
20287c0e764SRichard Cochran 
20387c0e764SRichard Cochran static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
20487c0e764SRichard Cochran {
20587c0e764SRichard Cochran 	unsigned long flags;
20687c0e764SRichard Cochran 	struct cpts *cpts = container_of(ptp, struct cpts, info);
20787c0e764SRichard Cochran 
20887c0e764SRichard Cochran 	spin_lock_irqsave(&cpts->lock, flags);
209f25a30beSRichard Cochran 	timecounter_adjtime(&cpts->tc, delta);
21087c0e764SRichard Cochran 	spin_unlock_irqrestore(&cpts->lock, flags);
21187c0e764SRichard Cochran 
21287c0e764SRichard Cochran 	return 0;
21387c0e764SRichard Cochran }
21487c0e764SRichard Cochran 
215856e59abSGrygorii Strashko static int cpts_ptp_gettimeex(struct ptp_clock_info *ptp,
216856e59abSGrygorii Strashko 			      struct timespec64 *ts,
217856e59abSGrygorii Strashko 			      struct ptp_system_timestamp *sts)
21887c0e764SRichard Cochran {
21987c0e764SRichard Cochran 	struct cpts *cpts = container_of(ptp, struct cpts, info);
220856e59abSGrygorii Strashko 	unsigned long flags;
221856e59abSGrygorii Strashko 	u64 ns;
22287c0e764SRichard Cochran 
22387c0e764SRichard Cochran 	spin_lock_irqsave(&cpts->lock, flags);
224e66dccceSGrygorii Strashko 
225856e59abSGrygorii Strashko 	cpts_update_cur_time(cpts, CPTS_EV_PUSH, sts);
226e66dccceSGrygorii Strashko 
22787c0e764SRichard Cochran 	ns = timecounter_read(&cpts->tc);
22887c0e764SRichard Cochran 	spin_unlock_irqrestore(&cpts->lock, flags);
22987c0e764SRichard Cochran 
23084d923ceSRichard Cochran 	*ts = ns_to_timespec64(ns);
23187c0e764SRichard Cochran 
23287c0e764SRichard Cochran 	return 0;
23387c0e764SRichard Cochran }
23487c0e764SRichard Cochran 
23587c0e764SRichard Cochran static int cpts_ptp_settime(struct ptp_clock_info *ptp,
236a5c79c26SRichard Cochran 			    const struct timespec64 *ts)
23787c0e764SRichard Cochran {
23887c0e764SRichard Cochran 	u64 ns;
23987c0e764SRichard Cochran 	unsigned long flags;
24087c0e764SRichard Cochran 	struct cpts *cpts = container_of(ptp, struct cpts, info);
24187c0e764SRichard Cochran 
24284d923ceSRichard Cochran 	ns = timespec64_to_ns(ts);
24387c0e764SRichard Cochran 
24487c0e764SRichard Cochran 	spin_lock_irqsave(&cpts->lock, flags);
24587c0e764SRichard Cochran 	timecounter_init(&cpts->tc, &cpts->cc, ns);
24687c0e764SRichard Cochran 	spin_unlock_irqrestore(&cpts->lock, flags);
24787c0e764SRichard Cochran 
24887c0e764SRichard Cochran 	return 0;
24987c0e764SRichard Cochran }
25087c0e764SRichard Cochran 
25187c0e764SRichard Cochran static int cpts_ptp_enable(struct ptp_clock_info *ptp,
25287c0e764SRichard Cochran 			   struct ptp_clock_request *rq, int on)
25387c0e764SRichard Cochran {
25487c0e764SRichard Cochran 	return -EOPNOTSUPP;
25587c0e764SRichard Cochran }
25687c0e764SRichard Cochran 
257*c8f8e47eSGrygorii Strashko static bool cpts_match_tx_ts(struct cpts *cpts, struct cpts_event *event)
258*c8f8e47eSGrygorii Strashko {
259*c8f8e47eSGrygorii Strashko 	struct sk_buff_head txq_list;
260*c8f8e47eSGrygorii Strashko 	struct sk_buff *skb, *tmp;
261*c8f8e47eSGrygorii Strashko 	unsigned long flags;
262*c8f8e47eSGrygorii Strashko 	bool found = false;
263*c8f8e47eSGrygorii Strashko 	u32 mtype_seqid;
264*c8f8e47eSGrygorii Strashko 
265*c8f8e47eSGrygorii Strashko 	mtype_seqid = event->high &
266*c8f8e47eSGrygorii Strashko 		      ((MESSAGE_TYPE_MASK << MESSAGE_TYPE_SHIFT) |
267*c8f8e47eSGrygorii Strashko 		       (SEQUENCE_ID_MASK << SEQUENCE_ID_SHIFT) |
268*c8f8e47eSGrygorii Strashko 		       (EVENT_TYPE_MASK << EVENT_TYPE_SHIFT));
269*c8f8e47eSGrygorii Strashko 
270*c8f8e47eSGrygorii Strashko 	__skb_queue_head_init(&txq_list);
271*c8f8e47eSGrygorii Strashko 
272*c8f8e47eSGrygorii Strashko 	spin_lock_irqsave(&cpts->txq.lock, flags);
273*c8f8e47eSGrygorii Strashko 	skb_queue_splice_init(&cpts->txq, &txq_list);
274*c8f8e47eSGrygorii Strashko 	spin_unlock_irqrestore(&cpts->txq.lock, flags);
275*c8f8e47eSGrygorii Strashko 
276*c8f8e47eSGrygorii Strashko 	skb_queue_walk_safe(&txq_list, skb, tmp) {
277*c8f8e47eSGrygorii Strashko 		struct skb_shared_hwtstamps ssh;
278*c8f8e47eSGrygorii Strashko 		struct cpts_skb_cb_data *skb_cb =
279*c8f8e47eSGrygorii Strashko 					(struct cpts_skb_cb_data *)skb->cb;
280*c8f8e47eSGrygorii Strashko 
281*c8f8e47eSGrygorii Strashko 		if (mtype_seqid == skb_cb->skb_mtype_seqid) {
282*c8f8e47eSGrygorii Strashko 			memset(&ssh, 0, sizeof(ssh));
283*c8f8e47eSGrygorii Strashko 			ssh.hwtstamp = ns_to_ktime(event->timestamp);
284*c8f8e47eSGrygorii Strashko 			skb_tstamp_tx(skb, &ssh);
285*c8f8e47eSGrygorii Strashko 			found = true;
286*c8f8e47eSGrygorii Strashko 			__skb_unlink(skb, &txq_list);
287*c8f8e47eSGrygorii Strashko 			dev_consume_skb_any(skb);
288*c8f8e47eSGrygorii Strashko 			dev_dbg(cpts->dev, "match tx timestamp mtype_seqid %08x\n",
289*c8f8e47eSGrygorii Strashko 				mtype_seqid);
290*c8f8e47eSGrygorii Strashko 			break;
291*c8f8e47eSGrygorii Strashko 		}
292*c8f8e47eSGrygorii Strashko 
293*c8f8e47eSGrygorii Strashko 		if (time_after(jiffies, skb_cb->tmo)) {
294*c8f8e47eSGrygorii Strashko 			/* timeout any expired skbs over 1s */
295*c8f8e47eSGrygorii Strashko 			dev_dbg(cpts->dev, "expiring tx timestamp from txq\n");
296*c8f8e47eSGrygorii Strashko 			__skb_unlink(skb, &txq_list);
297*c8f8e47eSGrygorii Strashko 			dev_consume_skb_any(skb);
298*c8f8e47eSGrygorii Strashko 		}
299*c8f8e47eSGrygorii Strashko 	}
300*c8f8e47eSGrygorii Strashko 
301*c8f8e47eSGrygorii Strashko 	spin_lock_irqsave(&cpts->txq.lock, flags);
302*c8f8e47eSGrygorii Strashko 	skb_queue_splice(&txq_list, &cpts->txq);
303*c8f8e47eSGrygorii Strashko 	spin_unlock_irqrestore(&cpts->txq.lock, flags);
304*c8f8e47eSGrygorii Strashko 
305*c8f8e47eSGrygorii Strashko 	return found;
306*c8f8e47eSGrygorii Strashko }
307*c8f8e47eSGrygorii Strashko 
308*c8f8e47eSGrygorii Strashko static void cpts_process_events(struct cpts *cpts)
309*c8f8e47eSGrygorii Strashko {
310*c8f8e47eSGrygorii Strashko 	struct list_head *this, *next;
311*c8f8e47eSGrygorii Strashko 	struct cpts_event *event;
312*c8f8e47eSGrygorii Strashko 	LIST_HEAD(events_free);
313*c8f8e47eSGrygorii Strashko 	unsigned long flags;
314*c8f8e47eSGrygorii Strashko 	LIST_HEAD(events);
315*c8f8e47eSGrygorii Strashko 
316*c8f8e47eSGrygorii Strashko 	spin_lock_irqsave(&cpts->lock, flags);
317*c8f8e47eSGrygorii Strashko 	list_splice_init(&cpts->events, &events);
318*c8f8e47eSGrygorii Strashko 	spin_unlock_irqrestore(&cpts->lock, flags);
319*c8f8e47eSGrygorii Strashko 
320*c8f8e47eSGrygorii Strashko 	list_for_each_safe(this, next, &events) {
321*c8f8e47eSGrygorii Strashko 		event = list_entry(this, struct cpts_event, list);
322*c8f8e47eSGrygorii Strashko 		if (cpts_match_tx_ts(cpts, event) ||
323*c8f8e47eSGrygorii Strashko 		    time_after(jiffies, event->tmo)) {
324*c8f8e47eSGrygorii Strashko 			list_del_init(&event->list);
325*c8f8e47eSGrygorii Strashko 			list_add(&event->list, &events_free);
326*c8f8e47eSGrygorii Strashko 		}
327*c8f8e47eSGrygorii Strashko 	}
328*c8f8e47eSGrygorii Strashko 
329*c8f8e47eSGrygorii Strashko 	spin_lock_irqsave(&cpts->lock, flags);
330*c8f8e47eSGrygorii Strashko 	list_splice_tail(&events, &cpts->events);
331*c8f8e47eSGrygorii Strashko 	list_splice_tail(&events_free, &cpts->pool);
332*c8f8e47eSGrygorii Strashko 	spin_unlock_irqrestore(&cpts->lock, flags);
333*c8f8e47eSGrygorii Strashko }
334*c8f8e47eSGrygorii Strashko 
335999f1292SGrygorii Strashko static long cpts_overflow_check(struct ptp_clock_info *ptp)
336999f1292SGrygorii Strashko {
337999f1292SGrygorii Strashko 	struct cpts *cpts = container_of(ptp, struct cpts, info);
338999f1292SGrygorii Strashko 	unsigned long delay = cpts->ov_check_period;
3390d5f54feSGrygorii Strashko 	unsigned long flags;
340e66dccceSGrygorii Strashko 	u64 ns;
341999f1292SGrygorii Strashko 
3420d5f54feSGrygorii Strashko 	spin_lock_irqsave(&cpts->lock, flags);
343856e59abSGrygorii Strashko 	cpts_update_cur_time(cpts, -1, NULL);
344*c8f8e47eSGrygorii Strashko 	spin_unlock_irqrestore(&cpts->lock, flags);
345e66dccceSGrygorii Strashko 
346e66dccceSGrygorii Strashko 	ns = timecounter_read(&cpts->tc);
3470d5f54feSGrygorii Strashko 
348*c8f8e47eSGrygorii Strashko 	cpts_process_events(cpts);
349*c8f8e47eSGrygorii Strashko 
350*c8f8e47eSGrygorii Strashko 	spin_lock_irqsave(&cpts->txq.lock, flags);
351f19dcd5fSIvan Khoronzhuk 	if (!skb_queue_empty(&cpts->txq)) {
352f19dcd5fSIvan Khoronzhuk 		cpts_purge_txq(cpts);
3530d5f54feSGrygorii Strashko 		if (!skb_queue_empty(&cpts->txq))
3540d5f54feSGrygorii Strashko 			delay = CPTS_SKB_TX_WORK_TIMEOUT;
355f19dcd5fSIvan Khoronzhuk 	}
356*c8f8e47eSGrygorii Strashko 	spin_unlock_irqrestore(&cpts->txq.lock, flags);
3570d5f54feSGrygorii Strashko 
358e66dccceSGrygorii Strashko 	dev_dbg(cpts->dev, "cpts overflow check at %lld\n", ns);
359999f1292SGrygorii Strashko 	return (long)delay;
360999f1292SGrygorii Strashko }
361999f1292SGrygorii Strashko 
362b6d08bd8SBhumika Goyal static const struct ptp_clock_info cpts_info = {
36387c0e764SRichard Cochran 	.owner		= THIS_MODULE,
36487c0e764SRichard Cochran 	.name		= "CTPS timer",
36587c0e764SRichard Cochran 	.max_adj	= 1000000,
36687c0e764SRichard Cochran 	.n_ext_ts	= 0,
3674986b4f0SRichard Cochran 	.n_pins		= 0,
36887c0e764SRichard Cochran 	.pps		= 0,
36987c0e764SRichard Cochran 	.adjfreq	= cpts_ptp_adjfreq,
37087c0e764SRichard Cochran 	.adjtime	= cpts_ptp_adjtime,
371856e59abSGrygorii Strashko 	.gettimex64	= cpts_ptp_gettimeex,
372a5c79c26SRichard Cochran 	.settime64	= cpts_ptp_settime,
37387c0e764SRichard Cochran 	.enable		= cpts_ptp_enable,
374999f1292SGrygorii Strashko 	.do_aux_work	= cpts_overflow_check,
37587c0e764SRichard Cochran };
37687c0e764SRichard Cochran 
3773bfd41b5SGrygorii Strashko static int cpts_skb_get_mtype_seqid(struct sk_buff *skb, u32 *mtype_seqid)
37887c0e764SRichard Cochran {
3793bfd41b5SGrygorii Strashko 	unsigned int ptp_class = ptp_classify_raw(skb);
38087c0e764SRichard Cochran 	u8 *msgtype, *data = skb->data;
3813bfd41b5SGrygorii Strashko 	unsigned int offset = 0;
3823bfd41b5SGrygorii Strashko 	u16 *seqid;
3833bfd41b5SGrygorii Strashko 
3843bfd41b5SGrygorii Strashko 	if (ptp_class == PTP_CLASS_NONE)
3853bfd41b5SGrygorii Strashko 		return 0;
38687c0e764SRichard Cochran 
387ae5c6c6dSStefan Sørensen 	if (ptp_class & PTP_CLASS_VLAN)
388ae5c6c6dSStefan Sørensen 		offset += VLAN_HLEN;
389ae5c6c6dSStefan Sørensen 
390ae5c6c6dSStefan Sørensen 	switch (ptp_class & PTP_CLASS_PMASK) {
391ae5c6c6dSStefan Sørensen 	case PTP_CLASS_IPV4:
392cca04b28SRichard Cochran 		offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
39387c0e764SRichard Cochran 		break;
394ae5c6c6dSStefan Sørensen 	case PTP_CLASS_IPV6:
395ae5c6c6dSStefan Sørensen 		offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
39687c0e764SRichard Cochran 		break;
397ae5c6c6dSStefan Sørensen 	case PTP_CLASS_L2:
398ae5c6c6dSStefan Sørensen 		offset += ETH_HLEN;
39987c0e764SRichard Cochran 		break;
40087c0e764SRichard Cochran 	default:
40187c0e764SRichard Cochran 		return 0;
40287c0e764SRichard Cochran 	}
40387c0e764SRichard Cochran 
40487c0e764SRichard Cochran 	if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
40587c0e764SRichard Cochran 		return 0;
40687c0e764SRichard Cochran 
40787c0e764SRichard Cochran 	if (unlikely(ptp_class & PTP_CLASS_V1))
40887c0e764SRichard Cochran 		msgtype = data + offset + OFF_PTP_CONTROL;
40987c0e764SRichard Cochran 	else
41087c0e764SRichard Cochran 		msgtype = data + offset;
41187c0e764SRichard Cochran 
41287c0e764SRichard Cochran 	seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
4133bfd41b5SGrygorii Strashko 	*mtype_seqid = (*msgtype & MESSAGE_TYPE_MASK) << MESSAGE_TYPE_SHIFT;
4143bfd41b5SGrygorii Strashko 	*mtype_seqid |= (ntohs(*seqid) & SEQUENCE_ID_MASK) << SEQUENCE_ID_SHIFT;
41587c0e764SRichard Cochran 
4163bfd41b5SGrygorii Strashko 	return 1;
41787c0e764SRichard Cochran }
41887c0e764SRichard Cochran 
4193bfd41b5SGrygorii Strashko static u64 cpts_find_ts(struct cpts *cpts, struct sk_buff *skb,
4203bfd41b5SGrygorii Strashko 			int ev_type, u32 skb_mtype_seqid)
42187c0e764SRichard Cochran {
42287c0e764SRichard Cochran 	struct list_head *this, *next;
4233bfd41b5SGrygorii Strashko 	struct cpts_event *event;
42487c0e764SRichard Cochran 	unsigned long flags;
4253bfd41b5SGrygorii Strashko 	u32 mtype_seqid;
4263bfd41b5SGrygorii Strashko 	u64 ns = 0;
42787c0e764SRichard Cochran 
42887c0e764SRichard Cochran 	spin_lock_irqsave(&cpts->lock, flags);
429a93439ccSGrygorii Strashko 	cpts_fifo_read(cpts, -1);
43087c0e764SRichard Cochran 	list_for_each_safe(this, next, &cpts->events) {
43187c0e764SRichard Cochran 		event = list_entry(this, struct cpts_event, list);
43287c0e764SRichard Cochran 		if (event_expired(event)) {
43387c0e764SRichard Cochran 			list_del_init(&event->list);
43487c0e764SRichard Cochran 			list_add(&event->list, &cpts->pool);
43587c0e764SRichard Cochran 			continue;
43687c0e764SRichard Cochran 		}
4373bfd41b5SGrygorii Strashko 
4383bfd41b5SGrygorii Strashko 		mtype_seqid = event->high &
4393bfd41b5SGrygorii Strashko 			      ((MESSAGE_TYPE_MASK << MESSAGE_TYPE_SHIFT) |
4403bfd41b5SGrygorii Strashko 			       (SEQUENCE_ID_MASK << SEQUENCE_ID_SHIFT) |
4413bfd41b5SGrygorii Strashko 			       (EVENT_TYPE_MASK << EVENT_TYPE_SHIFT));
4423bfd41b5SGrygorii Strashko 
4433bfd41b5SGrygorii Strashko 		if (mtype_seqid == skb_mtype_seqid) {
444e66dccceSGrygorii Strashko 			ns = event->timestamp;
44587c0e764SRichard Cochran 			list_del_init(&event->list);
44687c0e764SRichard Cochran 			list_add(&event->list, &cpts->pool);
44787c0e764SRichard Cochran 			break;
44887c0e764SRichard Cochran 		}
44987c0e764SRichard Cochran 	}
45087c0e764SRichard Cochran 	spin_unlock_irqrestore(&cpts->lock, flags);
45187c0e764SRichard Cochran 
45287c0e764SRichard Cochran 	return ns;
45387c0e764SRichard Cochran }
45487c0e764SRichard Cochran 
45587c0e764SRichard Cochran void cpts_rx_timestamp(struct cpts *cpts, struct sk_buff *skb)
45687c0e764SRichard Cochran {
4573bfd41b5SGrygorii Strashko 	struct cpts_skb_cb_data *skb_cb = (struct cpts_skb_cb_data *)skb->cb;
45887c0e764SRichard Cochran 	struct skb_shared_hwtstamps *ssh;
4593bfd41b5SGrygorii Strashko 	int ret;
4603bfd41b5SGrygorii Strashko 	u64 ns;
46187c0e764SRichard Cochran 
4623bfd41b5SGrygorii Strashko 	ret = cpts_skb_get_mtype_seqid(skb, &skb_cb->skb_mtype_seqid);
4633bfd41b5SGrygorii Strashko 	if (!ret)
4643bfd41b5SGrygorii Strashko 		return;
4653bfd41b5SGrygorii Strashko 
4663bfd41b5SGrygorii Strashko 	skb_cb->skb_mtype_seqid |= (CPTS_EV_RX << EVENT_TYPE_SHIFT);
4673bfd41b5SGrygorii Strashko 
4683bfd41b5SGrygorii Strashko 	dev_dbg(cpts->dev, "%s mtype seqid %08x\n",
4693bfd41b5SGrygorii Strashko 		__func__, skb_cb->skb_mtype_seqid);
4703bfd41b5SGrygorii Strashko 
4713bfd41b5SGrygorii Strashko 	ns = cpts_find_ts(cpts, skb, CPTS_EV_RX, skb_cb->skb_mtype_seqid);
47287c0e764SRichard Cochran 	if (!ns)
47387c0e764SRichard Cochran 		return;
47487c0e764SRichard Cochran 	ssh = skb_hwtstamps(skb);
47587c0e764SRichard Cochran 	memset(ssh, 0, sizeof(*ssh));
47687c0e764SRichard Cochran 	ssh->hwtstamp = ns_to_ktime(ns);
47787c0e764SRichard Cochran }
478c8395d4eSGrygorii Strashko EXPORT_SYMBOL_GPL(cpts_rx_timestamp);
47987c0e764SRichard Cochran 
48087c0e764SRichard Cochran void cpts_tx_timestamp(struct cpts *cpts, struct sk_buff *skb)
48187c0e764SRichard Cochran {
4823bfd41b5SGrygorii Strashko 	struct cpts_skb_cb_data *skb_cb = (struct cpts_skb_cb_data *)skb->cb;
4833bfd41b5SGrygorii Strashko 	int ret;
48487c0e764SRichard Cochran 
48587c0e764SRichard Cochran 	if (!(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS))
48687c0e764SRichard Cochran 		return;
4873bfd41b5SGrygorii Strashko 
4883bfd41b5SGrygorii Strashko 	ret = cpts_skb_get_mtype_seqid(skb, &skb_cb->skb_mtype_seqid);
4893bfd41b5SGrygorii Strashko 	if (!ret)
4903bfd41b5SGrygorii Strashko 		return;
4913bfd41b5SGrygorii Strashko 
4923bfd41b5SGrygorii Strashko 	skb_cb->skb_mtype_seqid |= (CPTS_EV_TX << EVENT_TYPE_SHIFT);
4933bfd41b5SGrygorii Strashko 
4943bfd41b5SGrygorii Strashko 	dev_dbg(cpts->dev, "%s mtype seqid %08x\n",
4953bfd41b5SGrygorii Strashko 		__func__, skb_cb->skb_mtype_seqid);
4963bfd41b5SGrygorii Strashko 
497*c8f8e47eSGrygorii Strashko 	/* Always defer TX TS processing to PTP worker */
498*c8f8e47eSGrygorii Strashko 	skb_get(skb);
499*c8f8e47eSGrygorii Strashko 	/* get the timestamp for timeouts */
500*c8f8e47eSGrygorii Strashko 	skb_cb->tmo = jiffies + msecs_to_jiffies(CPTS_SKB_RX_TX_TMO);
501*c8f8e47eSGrygorii Strashko 	skb_queue_tail(&cpts->txq, skb);
502*c8f8e47eSGrygorii Strashko 	ptp_schedule_worker(cpts->clock, 0);
50387c0e764SRichard Cochran }
504c8395d4eSGrygorii Strashko EXPORT_SYMBOL_GPL(cpts_tx_timestamp);
50587c0e764SRichard Cochran 
5068a2c9a5aSGrygorii Strashko int cpts_register(struct cpts *cpts)
50787c0e764SRichard Cochran {
50887c0e764SRichard Cochran 	int err, i;
50987c0e764SRichard Cochran 
5100d5f54feSGrygorii Strashko 	skb_queue_head_init(&cpts->txq);
51187c0e764SRichard Cochran 	INIT_LIST_HEAD(&cpts->events);
51287c0e764SRichard Cochran 	INIT_LIST_HEAD(&cpts->pool);
51387c0e764SRichard Cochran 	for (i = 0; i < CPTS_MAX_EVENTS; i++)
51487c0e764SRichard Cochran 		list_add(&cpts->pool_data[i].list, &cpts->pool);
51587c0e764SRichard Cochran 
5168a2c9a5aSGrygorii Strashko 	clk_enable(cpts->refclk);
5178a2c9a5aSGrygorii Strashko 
51887c0e764SRichard Cochran 	cpts_write32(cpts, CPTS_EN, control);
51987c0e764SRichard Cochran 	cpts_write32(cpts, TS_PEND_EN, int_enable);
52087c0e764SRichard Cochran 
521693bd8b7SIvan Khoronzhuk 	timecounter_init(&cpts->tc, &cpts->cc, ktime_get_real_ns());
52287c0e764SRichard Cochran 
5238a2c9a5aSGrygorii Strashko 	cpts->clock = ptp_clock_register(&cpts->info, cpts->dev);
5246c691405SGrygorii Strashko 	if (IS_ERR(cpts->clock)) {
5256c691405SGrygorii Strashko 		err = PTR_ERR(cpts->clock);
5266c691405SGrygorii Strashko 		cpts->clock = NULL;
5276c691405SGrygorii Strashko 		goto err_ptp;
5286c691405SGrygorii Strashko 	}
5296c691405SGrygorii Strashko 	cpts->phc_index = ptp_clock_index(cpts->clock);
5306c691405SGrygorii Strashko 
531999f1292SGrygorii Strashko 	ptp_schedule_worker(cpts->clock, cpts->ov_check_period);
53287c0e764SRichard Cochran 	return 0;
5336c691405SGrygorii Strashko 
5346c691405SGrygorii Strashko err_ptp:
5358a2c9a5aSGrygorii Strashko 	clk_disable(cpts->refclk);
5366c691405SGrygorii Strashko 	return err;
53787c0e764SRichard Cochran }
538c8395d4eSGrygorii Strashko EXPORT_SYMBOL_GPL(cpts_register);
53987c0e764SRichard Cochran 
54087c0e764SRichard Cochran void cpts_unregister(struct cpts *cpts)
54187c0e764SRichard Cochran {
5428a2c9a5aSGrygorii Strashko 	if (WARN_ON(!cpts->clock))
5438a2c9a5aSGrygorii Strashko 		return;
5448a2c9a5aSGrygorii Strashko 
5458a2c9a5aSGrygorii Strashko 	ptp_clock_unregister(cpts->clock);
5468a2c9a5aSGrygorii Strashko 	cpts->clock = NULL;
5478fcd6891SGrygorii Strashko 
5488fcd6891SGrygorii Strashko 	cpts_write32(cpts, 0, int_enable);
5498fcd6891SGrygorii Strashko 	cpts_write32(cpts, 0, control);
5508fcd6891SGrygorii Strashko 
5510d5f54feSGrygorii Strashko 	/* Drop all packet */
5520d5f54feSGrygorii Strashko 	skb_queue_purge(&cpts->txq);
5530d5f54feSGrygorii Strashko 
5548a2c9a5aSGrygorii Strashko 	clk_disable(cpts->refclk);
55587c0e764SRichard Cochran }
556c8395d4eSGrygorii Strashko EXPORT_SYMBOL_GPL(cpts_unregister);
557c8395d4eSGrygorii Strashko 
55888f0f0b0SGrygorii Strashko static void cpts_calc_mult_shift(struct cpts *cpts)
55988f0f0b0SGrygorii Strashko {
56088f0f0b0SGrygorii Strashko 	u64 frac, maxsec, ns;
56188f0f0b0SGrygorii Strashko 	u32 freq;
56288f0f0b0SGrygorii Strashko 
56388f0f0b0SGrygorii Strashko 	freq = clk_get_rate(cpts->refclk);
56488f0f0b0SGrygorii Strashko 
56588f0f0b0SGrygorii Strashko 	/* Calc the maximum number of seconds which we can run before
56688f0f0b0SGrygorii Strashko 	 * wrapping around.
56788f0f0b0SGrygorii Strashko 	 */
56888f0f0b0SGrygorii Strashko 	maxsec = cpts->cc.mask;
56988f0f0b0SGrygorii Strashko 	do_div(maxsec, freq);
57088f0f0b0SGrygorii Strashko 	/* limit conversation rate to 10 sec as higher values will produce
57188f0f0b0SGrygorii Strashko 	 * too small mult factors and so reduce the conversion accuracy
57288f0f0b0SGrygorii Strashko 	 */
57388f0f0b0SGrygorii Strashko 	if (maxsec > 10)
57488f0f0b0SGrygorii Strashko 		maxsec = 10;
57588f0f0b0SGrygorii Strashko 
57620138cf9SGrygorii Strashko 	/* Calc overflow check period (maxsec / 2) */
57720138cf9SGrygorii Strashko 	cpts->ov_check_period = (HZ * maxsec) / 2;
57820138cf9SGrygorii Strashko 	dev_info(cpts->dev, "cpts: overflow check period %lu (jiffies)\n",
57920138cf9SGrygorii Strashko 		 cpts->ov_check_period);
58020138cf9SGrygorii Strashko 
58188f0f0b0SGrygorii Strashko 	if (cpts->cc.mult || cpts->cc.shift)
58288f0f0b0SGrygorii Strashko 		return;
58388f0f0b0SGrygorii Strashko 
58488f0f0b0SGrygorii Strashko 	clocks_calc_mult_shift(&cpts->cc.mult, &cpts->cc.shift,
58588f0f0b0SGrygorii Strashko 			       freq, NSEC_PER_SEC, maxsec);
58688f0f0b0SGrygorii Strashko 
58788f0f0b0SGrygorii Strashko 	frac = 0;
58888f0f0b0SGrygorii Strashko 	ns = cyclecounter_cyc2ns(&cpts->cc, freq, cpts->cc.mask, &frac);
58988f0f0b0SGrygorii Strashko 
59088f0f0b0SGrygorii Strashko 	dev_info(cpts->dev,
59188f0f0b0SGrygorii Strashko 		 "CPTS: ref_clk_freq:%u calc_mult:%u calc_shift:%u error:%lld nsec/sec\n",
59288f0f0b0SGrygorii Strashko 		 freq, cpts->cc.mult, cpts->cc.shift, (ns - NSEC_PER_SEC));
59388f0f0b0SGrygorii Strashko }
59488f0f0b0SGrygorii Strashko 
595a3047a81SGrygorii Strashko static int cpts_of_mux_clk_setup(struct cpts *cpts, struct device_node *node)
596a3047a81SGrygorii Strashko {
597a3047a81SGrygorii Strashko 	struct device_node *refclk_np;
598a3047a81SGrygorii Strashko 	const char **parent_names;
599a3047a81SGrygorii Strashko 	unsigned int num_parents;
600a3047a81SGrygorii Strashko 	struct clk_hw *clk_hw;
601a3047a81SGrygorii Strashko 	int ret = -EINVAL;
602a3047a81SGrygorii Strashko 	u32 *mux_table;
603a3047a81SGrygorii Strashko 
604a3047a81SGrygorii Strashko 	refclk_np = of_get_child_by_name(node, "cpts-refclk-mux");
605a3047a81SGrygorii Strashko 	if (!refclk_np)
606a3047a81SGrygorii Strashko 		/* refclk selection supported not for all SoCs */
607a3047a81SGrygorii Strashko 		return 0;
608a3047a81SGrygorii Strashko 
609a3047a81SGrygorii Strashko 	num_parents = of_clk_get_parent_count(refclk_np);
610a3047a81SGrygorii Strashko 	if (num_parents < 1) {
611a3047a81SGrygorii Strashko 		dev_err(cpts->dev, "mux-clock %s must have parents\n",
612a3047a81SGrygorii Strashko 			refclk_np->name);
613a3047a81SGrygorii Strashko 		goto mux_fail;
614a3047a81SGrygorii Strashko 	}
615a3047a81SGrygorii Strashko 
616a3047a81SGrygorii Strashko 	parent_names = devm_kzalloc(cpts->dev, (sizeof(char *) * num_parents),
617a3047a81SGrygorii Strashko 				    GFP_KERNEL);
618a3047a81SGrygorii Strashko 
619a3047a81SGrygorii Strashko 	mux_table = devm_kzalloc(cpts->dev, sizeof(*mux_table) * num_parents,
620a3047a81SGrygorii Strashko 				 GFP_KERNEL);
621a3047a81SGrygorii Strashko 	if (!mux_table || !parent_names) {
622a3047a81SGrygorii Strashko 		ret = -ENOMEM;
623a3047a81SGrygorii Strashko 		goto mux_fail;
624a3047a81SGrygorii Strashko 	}
625a3047a81SGrygorii Strashko 
626a3047a81SGrygorii Strashko 	of_clk_parent_fill(refclk_np, parent_names, num_parents);
627a3047a81SGrygorii Strashko 
628a3047a81SGrygorii Strashko 	ret = of_property_read_variable_u32_array(refclk_np, "ti,mux-tbl",
629a3047a81SGrygorii Strashko 						  mux_table,
630a3047a81SGrygorii Strashko 						  num_parents, num_parents);
631a3047a81SGrygorii Strashko 	if (ret < 0)
632a3047a81SGrygorii Strashko 		goto mux_fail;
633a3047a81SGrygorii Strashko 
634a3047a81SGrygorii Strashko 	clk_hw = clk_hw_register_mux_table(cpts->dev, refclk_np->name,
635a3047a81SGrygorii Strashko 					   parent_names, num_parents,
636a3047a81SGrygorii Strashko 					   0,
637a3047a81SGrygorii Strashko 					   &cpts->reg->rftclk_sel, 0, 0x1F,
638a3047a81SGrygorii Strashko 					   0, mux_table, NULL);
639a3047a81SGrygorii Strashko 	if (IS_ERR(clk_hw)) {
640a3047a81SGrygorii Strashko 		ret = PTR_ERR(clk_hw);
641a3047a81SGrygorii Strashko 		goto mux_fail;
642a3047a81SGrygorii Strashko 	}
643a3047a81SGrygorii Strashko 
644a3047a81SGrygorii Strashko 	ret = devm_add_action_or_reset(cpts->dev,
645a3047a81SGrygorii Strashko 				       (void(*)(void *))clk_hw_unregister_mux,
646a3047a81SGrygorii Strashko 				       clk_hw);
647a3047a81SGrygorii Strashko 	if (ret) {
648a3047a81SGrygorii Strashko 		dev_err(cpts->dev, "add clkmux unreg action %d", ret);
649a3047a81SGrygorii Strashko 		goto mux_fail;
650a3047a81SGrygorii Strashko 	}
651a3047a81SGrygorii Strashko 
652a3047a81SGrygorii Strashko 	ret = of_clk_add_hw_provider(refclk_np, of_clk_hw_simple_get, clk_hw);
653a3047a81SGrygorii Strashko 	if (ret)
654a3047a81SGrygorii Strashko 		goto mux_fail;
655a3047a81SGrygorii Strashko 
656a3047a81SGrygorii Strashko 	ret = devm_add_action_or_reset(cpts->dev,
657a3047a81SGrygorii Strashko 				       (void(*)(void *))of_clk_del_provider,
658a3047a81SGrygorii Strashko 				       refclk_np);
659a3047a81SGrygorii Strashko 	if (ret) {
660a3047a81SGrygorii Strashko 		dev_err(cpts->dev, "add clkmux provider unreg action %d", ret);
661a3047a81SGrygorii Strashko 		goto mux_fail;
662a3047a81SGrygorii Strashko 	}
663a3047a81SGrygorii Strashko 
664a3047a81SGrygorii Strashko 	return ret;
665a3047a81SGrygorii Strashko 
666a3047a81SGrygorii Strashko mux_fail:
667a3047a81SGrygorii Strashko 	of_node_put(refclk_np);
668a3047a81SGrygorii Strashko 	return ret;
669a3047a81SGrygorii Strashko }
670a3047a81SGrygorii Strashko 
6714a88fb95SGrygorii Strashko static int cpts_of_parse(struct cpts *cpts, struct device_node *node)
6724a88fb95SGrygorii Strashko {
6734a88fb95SGrygorii Strashko 	int ret = -EINVAL;
6744a88fb95SGrygorii Strashko 	u32 prop;
6754a88fb95SGrygorii Strashko 
67688f0f0b0SGrygorii Strashko 	if (!of_property_read_u32(node, "cpts_clock_mult", &prop))
6774a88fb95SGrygorii Strashko 		cpts->cc.mult = prop;
6784a88fb95SGrygorii Strashko 
67988f0f0b0SGrygorii Strashko 	if (!of_property_read_u32(node, "cpts_clock_shift", &prop))
6804a88fb95SGrygorii Strashko 		cpts->cc.shift = prop;
6814a88fb95SGrygorii Strashko 
68288f0f0b0SGrygorii Strashko 	if ((cpts->cc.mult && !cpts->cc.shift) ||
68388f0f0b0SGrygorii Strashko 	    (!cpts->cc.mult && cpts->cc.shift))
68488f0f0b0SGrygorii Strashko 		goto of_error;
68588f0f0b0SGrygorii Strashko 
686a3047a81SGrygorii Strashko 	return cpts_of_mux_clk_setup(cpts, node);
6874a88fb95SGrygorii Strashko 
6884a88fb95SGrygorii Strashko of_error:
6894a88fb95SGrygorii Strashko 	dev_err(cpts->dev, "CPTS: Missing property in the DT.\n");
6904a88fb95SGrygorii Strashko 	return ret;
6914a88fb95SGrygorii Strashko }
6924a88fb95SGrygorii Strashko 
6938a2c9a5aSGrygorii Strashko struct cpts *cpts_create(struct device *dev, void __iomem *regs,
6944a88fb95SGrygorii Strashko 			 struct device_node *node)
6958a2c9a5aSGrygorii Strashko {
6968a2c9a5aSGrygorii Strashko 	struct cpts *cpts;
6974a88fb95SGrygorii Strashko 	int ret;
6988a2c9a5aSGrygorii Strashko 
6998a2c9a5aSGrygorii Strashko 	cpts = devm_kzalloc(dev, sizeof(*cpts), GFP_KERNEL);
7008a2c9a5aSGrygorii Strashko 	if (!cpts)
7018a2c9a5aSGrygorii Strashko 		return ERR_PTR(-ENOMEM);
7028a2c9a5aSGrygorii Strashko 
7038a2c9a5aSGrygorii Strashko 	cpts->dev = dev;
7048a2c9a5aSGrygorii Strashko 	cpts->reg = (struct cpsw_cpts __iomem *)regs;
7058a2c9a5aSGrygorii Strashko 	spin_lock_init(&cpts->lock);
7068a2c9a5aSGrygorii Strashko 
7074a88fb95SGrygorii Strashko 	ret = cpts_of_parse(cpts, node);
7084a88fb95SGrygorii Strashko 	if (ret)
7094a88fb95SGrygorii Strashko 		return ERR_PTR(ret);
7104a88fb95SGrygorii Strashko 
7118a6389a5SGrygorii Strashko 	cpts->refclk = devm_get_clk_from_child(dev, node, "cpts");
7128a6389a5SGrygorii Strashko 	if (IS_ERR(cpts->refclk))
7138a6389a5SGrygorii Strashko 		/* try get clk from dev node for compatibility */
7148a2c9a5aSGrygorii Strashko 		cpts->refclk = devm_clk_get(dev, "cpts");
7158a6389a5SGrygorii Strashko 
7168a2c9a5aSGrygorii Strashko 	if (IS_ERR(cpts->refclk)) {
7178a6389a5SGrygorii Strashko 		dev_err(dev, "Failed to get cpts refclk %ld\n",
7188a6389a5SGrygorii Strashko 			PTR_ERR(cpts->refclk));
719bde4c563SHernán Gonzalez 		return ERR_CAST(cpts->refclk);
7208a2c9a5aSGrygorii Strashko 	}
7218a2c9a5aSGrygorii Strashko 
7222d822f2dSKangjie Lu 	ret = clk_prepare(cpts->refclk);
7232d822f2dSKangjie Lu 	if (ret)
7242d822f2dSKangjie Lu 		return ERR_PTR(ret);
7258a2c9a5aSGrygorii Strashko 
7268a2c9a5aSGrygorii Strashko 	cpts->cc.read = cpts_systim_read;
7278a2c9a5aSGrygorii Strashko 	cpts->cc.mask = CLOCKSOURCE_MASK(32);
72888f0f0b0SGrygorii Strashko 	cpts->info = cpts_info;
72988f0f0b0SGrygorii Strashko 
73088f0f0b0SGrygorii Strashko 	cpts_calc_mult_shift(cpts);
7314a88fb95SGrygorii Strashko 	/* save cc.mult original value as it can be modified
7324a88fb95SGrygorii Strashko 	 * by cpts_ptp_adjfreq().
7334a88fb95SGrygorii Strashko 	 */
7344a88fb95SGrygorii Strashko 	cpts->cc_mult = cpts->cc.mult;
7358a2c9a5aSGrygorii Strashko 
7368a2c9a5aSGrygorii Strashko 	return cpts;
7378a2c9a5aSGrygorii Strashko }
7388a2c9a5aSGrygorii Strashko EXPORT_SYMBOL_GPL(cpts_create);
7398a2c9a5aSGrygorii Strashko 
7408a2c9a5aSGrygorii Strashko void cpts_release(struct cpts *cpts)
7418a2c9a5aSGrygorii Strashko {
7428a2c9a5aSGrygorii Strashko 	if (!cpts)
7438a2c9a5aSGrygorii Strashko 		return;
7448a2c9a5aSGrygorii Strashko 
7458a2c9a5aSGrygorii Strashko 	if (WARN_ON(!cpts->refclk))
7468a2c9a5aSGrygorii Strashko 		return;
7478a2c9a5aSGrygorii Strashko 
7488a2c9a5aSGrygorii Strashko 	clk_unprepare(cpts->refclk);
7498a2c9a5aSGrygorii Strashko }
7508a2c9a5aSGrygorii Strashko EXPORT_SYMBOL_GPL(cpts_release);
7518a2c9a5aSGrygorii Strashko 
752c8395d4eSGrygorii Strashko MODULE_LICENSE("GPL v2");
753c8395d4eSGrygorii Strashko MODULE_DESCRIPTION("TI CPTS driver");
754c8395d4eSGrygorii Strashko MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
755