~hc/RK356X_SDK_RELEASE.git

..	..	@@ -1,22 +1,11 @@
	1	+// SPDX-License-Identifier: GPL-2.0+
1	2	/*
2	3	* TI Common Platform Time Sync
3	4	*
4	5	* Copyright (C) 2012 Richard Cochran <richardcochran@gmail.com>
5	6	*
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	7	*/
	8	+#include <linux/clk-provider.h>
20	9	#include <linux/err.h>
21	10	#include <linux/if.h>
22	11	#include <linux/hrtimer.h>
..	..	@@ -32,16 +21,21 @@
32	21	#include "cpts.h"
33	22
34	23	#define CPTS_SKB_TX_WORK_TIMEOUT 1 /* jiffies */
	24	+#define CPTS_SKB_RX_TX_TMO 100 /ms /
	25	+#define CPTS_EVENT_RX_TX_TIMEOUT (100) /* ms */
35	26
36	27	struct cpts_skb_cb_data {
	28	+ u32 skb_mtype_seqid;
37	29	unsigned long tmo;
38	30	};
39	31
40	32	#define cpts_read32(c, r) readl_relaxed(&c->reg->r)
41	33	#define cpts_write32(c, v, r) writel_relaxed(v, &c->reg->r)
42	34
43		-static int cpts_match(struct sk_buff *skb, unsigned int ptp_class,
44		- u16 ts_seqid, u8 ts_msgtype);
	35	+static int cpts_event_port(struct cpts_event *event)
	36	+{
	37	+ return (event->high >> PORT_NUMBER_SHIFT) & PORT_NUMBER_MASK;
	38	+}
45	39
46	40	static int event_expired(struct cpts_event *event)
47	41	{
..	..	@@ -82,50 +76,27 @@
82	76	}
83	77
84	78	if (removed)
85		- pr_debug("cpts: event pool cleaned up %d\n", removed);
	79	+ dev_dbg(cpts->dev, "cpts: event pool cleaned up %d\n", removed);
86	80	return removed ? 0 : -1;
87	81	}
88	82
89		-static bool cpts_match_tx_ts(struct cpts cpts, struct cpts_event event)
	83	+static void cpts_purge_txq(struct cpts *cpts)
90	84	{
	85	+ struct cpts_skb_cb_data *skb_cb;
91	86	struct sk_buff skb, tmp;
92		- u16 seqid;
93		- u8 mtype;
94		- bool found = false;
	87	+ int removed = 0;
95	88
96		- mtype = (event->high >> MESSAGE_TYPE_SHIFT) & MESSAGE_TYPE_MASK;
97		- seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK;
98		-
99		- /* no need to grab txq.lock as access is always done under cpts->lock */
100	89	skb_queue_walk_safe(&cpts->txq, skb, tmp) {
101		- struct skb_shared_hwtstamps ssh;
102		- unsigned int class = ptp_classify_raw(skb);
103		- struct cpts_skb_cb_data *skb_cb =
104		- (struct cpts_skb_cb_data *)skb->cb;
105		-
106		- if (cpts_match(skb, class, seqid, mtype)) {
107		- u64 ns = timecounter_cyc2time(&cpts->tc, event->low);
108		-
109		- memset(&ssh, 0, sizeof(ssh));
110		- ssh.hwtstamp = ns_to_ktime(ns);
111		- skb_tstamp_tx(skb, &ssh);
112		- found = true;
113		- __skb_unlink(skb, &cpts->txq);
114		- dev_consume_skb_any(skb);
115		- dev_dbg(cpts->dev, "match tx timestamp mtype %u seqid %04x\n",
116		- mtype, seqid);
117		- break;
118		- }
119		-
	90	+ skb_cb = (struct cpts_skb_cb_data *)skb->cb;
120	91	if (time_after(jiffies, skb_cb->tmo)) {
121		- /* timeout any expired skbs over 1s */
122		- dev_dbg(cpts->dev, "expiring tx timestamp from txq\n");
123	92	__skb_unlink(skb, &cpts->txq);
124	93	dev_consume_skb_any(skb);
	94	+ ++removed;
125	95	}
126	96	}
127	97
128		- return found;
	98	+ if (removed)
	99	+ dev_dbg(cpts->dev, "txq cleaned up %d\n", removed);
129	100	}
130	101
131	102	/*
..	..	@@ -133,85 +104,121 @@
133	104	*/
134	105	static int cpts_fifo_read(struct cpts *cpts, int match)
135	106	{
	107	+ struct ptp_clock_event pevent;
	108	+ bool need_schedule = false;
	109	+ struct cpts_event *event;
	110	+ unsigned long flags;
136	111	int i, type = -1;
137	112	u32 hi, lo;
138		- struct cpts_event *event;
	113	+
	114	+ spin_lock_irqsave(&cpts->lock, flags);
139	115
140	116	for (i = 0; i < CPTS_FIFO_DEPTH; i++) {
141	117	if (cpts_fifo_pop(cpts, &hi, &lo))
142	118	break;
143	119
144	120	if (list_empty(&cpts->pool) && cpts_purge_events(cpts)) {
145		- pr_err("cpts: event pool empty\n");
146		- return -1;
	121	+ dev_warn(cpts->dev, "cpts: event pool empty\n");
	122	+ break;
147	123	}
148	124
149	125	event = list_first_entry(&cpts->pool, struct cpts_event, list);
150		- event->tmo = jiffies + 2;
151	126	event->high = hi;
152	127	event->low = lo;
	128	+ event->timestamp = timecounter_cyc2time(&cpts->tc, event->low);
153	129	type = event_type(event);
	130	+
	131	+ dev_dbg(cpts->dev, "CPTS_EV: %d high:%08X low:%08x\n",
	132	+ type, event->high, event->low);
154	133	switch (type) {
155		- case CPTS_EV_TX:
156		- if (cpts_match_tx_ts(cpts, event)) {
157		- /* if the new event matches an existing skb,
158		- * then don't queue it
159		- */
160		- break;
161		- }
162		- /* fall through */
163	134	case CPTS_EV_PUSH:
	135	+ WRITE_ONCE(cpts->cur_timestamp, lo);
	136	+ timecounter_read(&cpts->tc);
	137	+ if (cpts->mult_new) {
	138	+ cpts->cc.mult = cpts->mult_new;
	139	+ cpts->mult_new = 0;
	140	+ }
	141	+ if (!cpts->irq_poll)
	142	+ complete(&cpts->ts_push_complete);
	143	+ break;
	144	+ case CPTS_EV_TX:
164	145	case CPTS_EV_RX:
	146	+ event->tmo = jiffies +
	147	+ msecs_to_jiffies(CPTS_EVENT_RX_TX_TIMEOUT);
	148	+
165	149	list_del_init(&event->list);
166	150	list_add_tail(&event->list, &cpts->events);
	151	+ need_schedule = true;
167	152	break;
168	153	case CPTS_EV_ROLL:
169	154	case CPTS_EV_HALF:
	155	+ break;
170	156	case CPTS_EV_HW:
	157	+ pevent.timestamp = event->timestamp;
	158	+ pevent.type = PTP_CLOCK_EXTTS;
	159	+ pevent.index = cpts_event_port(event) - 1;
	160	+ ptp_clock_event(cpts->clock, &pevent);
171	161	break;
172	162	default:
173		- pr_err("cpts: unknown event type\n");
	163	+ dev_err(cpts->dev, "cpts: unknown event type\n");
174	164	break;
175	165	}
176	166	if (type == match)
177	167	break;
178	168	}
	169	+
	170	+ spin_unlock_irqrestore(&cpts->lock, flags);
	171	+
	172	+ if (!cpts->irq_poll && need_schedule)
	173	+ ptp_schedule_worker(cpts->clock, 0);
	174	+
179	175	return type == match ? 0 : -1;
180	176	}
181	177
	178	+void cpts_misc_interrupt(struct cpts *cpts)
	179	+{
	180	+ cpts_fifo_read(cpts, -1);
	181	+}
	182	+EXPORT_SYMBOL_GPL(cpts_misc_interrupt);
	183	+
182	184	static u64 cpts_systim_read(const struct cyclecounter *cc)
183	185	{
184		- u64 val = 0;
185		- struct cpts_event *event;
186		- struct list_head this, next;
187	186	struct cpts *cpts = container_of(cc, struct cpts, cc);
188	187
	188	+ return READ_ONCE(cpts->cur_timestamp);
	189	+}
	190	+
	191	+static void cpts_update_cur_time(struct cpts *cpts, int match,
	192	+ struct ptp_system_timestamp *sts)
	193	+{
	194	+ unsigned long flags;
	195	+
	196	+ reinit_completion(&cpts->ts_push_complete);
	197	+
	198	+ /* use spin_lock_irqsave() here as it has to run very fast */
	199	+ spin_lock_irqsave(&cpts->lock, flags);
	200	+ ptp_read_system_prets(sts);
189	201	cpts_write32(cpts, TS_PUSH, ts_push);
190		- if (cpts_fifo_read(cpts, CPTS_EV_PUSH))
191		- pr_err("cpts: unable to obtain a time stamp\n");
	202	+ cpts_read32(cpts, ts_push);
	203	+ ptp_read_system_postts(sts);
	204	+ spin_unlock_irqrestore(&cpts->lock, flags);
192	205
193		- list_for_each_safe(this, next, &cpts->events) {
194		- event = list_entry(this, struct cpts_event, list);
195		- if (event_type(event) == CPTS_EV_PUSH) {
196		- list_del_init(&event->list);
197		- list_add(&event->list, &cpts->pool);
198		- val = event->low;
199		- break;
200		- }
201		- }
	206	+ if (cpts->irq_poll && cpts_fifo_read(cpts, match) && match != -1)
	207	+ dev_err(cpts->dev, "cpts: unable to obtain a time stamp\n");
202	208
203		- return val;
	209	+ if (!cpts->irq_poll &&
	210	+ !wait_for_completion_timeout(&cpts->ts_push_complete, HZ))
	211	+ dev_err(cpts->dev, "cpts: obtain a time stamp timeout\n");
204	212	}
205	213
206	214	/* PTP clock operations */
207	215
208	216	static int cpts_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
209	217	{
210		- u64 adj;
211		- u32 diff, mult;
212		- int neg_adj = 0;
213		- unsigned long flags;
214	218	struct cpts *cpts = container_of(ptp, struct cpts, info);
	219	+ int neg_adj = 0;
	220	+ u32 diff, mult;
	221	+ u64 adj;
215	222
216	223	if (ppb < 0) {
217	224	neg_adj = 1;
..	..	@@ -222,38 +229,40 @@
222	229	adj *= ppb;
223	230	diff = div_u64(adj, 1000000000ULL);
224	231
225		- spin_lock_irqsave(&cpts->lock, flags);
	232	+ mutex_lock(&cpts->ptp_clk_mutex);
226	233
227		- timecounter_read(&cpts->tc);
	234	+ cpts->mult_new = neg_adj ? mult - diff : mult + diff;
228	235
229		- cpts->cc.mult = neg_adj ? mult - diff : mult + diff;
	236	+ cpts_update_cur_time(cpts, CPTS_EV_PUSH, NULL);
230	237
231		- spin_unlock_irqrestore(&cpts->lock, flags);
232		-
	238	+ mutex_unlock(&cpts->ptp_clk_mutex);
233	239	return 0;
234	240	}
235	241
236	242	static int cpts_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
237	243	{
238		- unsigned long flags;
239	244	struct cpts *cpts = container_of(ptp, struct cpts, info);
240	245
241		- spin_lock_irqsave(&cpts->lock, flags);
	246	+ mutex_lock(&cpts->ptp_clk_mutex);
242	247	timecounter_adjtime(&cpts->tc, delta);
243		- spin_unlock_irqrestore(&cpts->lock, flags);
	248	+ mutex_unlock(&cpts->ptp_clk_mutex);
244	249
245	250	return 0;
246	251	}
247	252
248		-static int cpts_ptp_gettime(struct ptp_clock_info ptp, struct timespec64 ts)
	253	+static int cpts_ptp_gettimeex(struct ptp_clock_info *ptp,
	254	+ struct timespec64 *ts,
	255	+ struct ptp_system_timestamp *sts)
249	256	{
250		- u64 ns;
251		- unsigned long flags;
252	257	struct cpts *cpts = container_of(ptp, struct cpts, info);
	258	+ u64 ns;
253	259
254		- spin_lock_irqsave(&cpts->lock, flags);
	260	+ mutex_lock(&cpts->ptp_clk_mutex);
	261	+
	262	+ cpts_update_cur_time(cpts, CPTS_EV_PUSH, sts);
	263	+
255	264	ns = timecounter_read(&cpts->tc);
256		- spin_unlock_irqrestore(&cpts->lock, flags);
	265	+ mutex_unlock(&cpts->ptp_clk_mutex);
257	266
258	267	*ts = ns_to_timespec64(ns);
259	268
..	..	@@ -263,15 +272,38 @@
263	272	static int cpts_ptp_settime(struct ptp_clock_info *ptp,
264	273	const struct timespec64 *ts)
265	274	{
266		- u64 ns;
267		- unsigned long flags;
268	275	struct cpts *cpts = container_of(ptp, struct cpts, info);
	276	+ u64 ns;
269	277
270	278	ns = timespec64_to_ns(ts);
271	279
272		- spin_lock_irqsave(&cpts->lock, flags);
	280	+ mutex_lock(&cpts->ptp_clk_mutex);
273	281	timecounter_init(&cpts->tc, &cpts->cc, ns);
274		- spin_unlock_irqrestore(&cpts->lock, flags);
	282	+ mutex_unlock(&cpts->ptp_clk_mutex);
	283	+
	284	+ return 0;
	285	+}
	286	+
	287	+static int cpts_extts_enable(struct cpts *cpts, u32 index, int on)
	288	+{
	289	+ u32 v;
	290	+
	291	+ if (((cpts->hw_ts_enable & BIT(index)) >> index) == on)
	292	+ return 0;
	293	+
	294	+ mutex_lock(&cpts->ptp_clk_mutex);
	295	+
	296	+ v = cpts_read32(cpts, control);
	297	+ if (on) {
	298	+ v \|= BIT(8 + index);
	299	+ cpts->hw_ts_enable \|= BIT(index);
	300	+ } else {
	301	+ v &= ~BIT(8 + index);
	302	+ cpts->hw_ts_enable &= ~BIT(index);
	303	+ }
	304	+ cpts_write32(cpts, v, control);
	305	+
	306	+ mutex_unlock(&cpts->ptp_clk_mutex);
275	307
276	308	return 0;
277	309	}
..	..	@@ -279,25 +311,120 @@
279	311	static int cpts_ptp_enable(struct ptp_clock_info *ptp,
280	312	struct ptp_clock_request *rq, int on)
281	313	{
	314	+ struct cpts *cpts = container_of(ptp, struct cpts, info);
	315	+
	316	+ switch (rq->type) {
	317	+ case PTP_CLK_REQ_EXTTS:
	318	+ return cpts_extts_enable(cpts, rq->extts.index, on);
	319	+ default:
	320	+ break;
	321	+ }
	322	+
282	323	return -EOPNOTSUPP;
	324	+}
	325	+
	326	+static bool cpts_match_tx_ts(struct cpts cpts, struct cpts_event event)
	327	+{
	328	+ struct sk_buff_head txq_list;
	329	+ struct sk_buff skb, tmp;
	330	+ unsigned long flags;
	331	+ bool found = false;
	332	+ u32 mtype_seqid;
	333	+
	334	+ mtype_seqid = event->high &
	335	+ ((MESSAGE_TYPE_MASK << MESSAGE_TYPE_SHIFT) \|
	336	+ (SEQUENCE_ID_MASK << SEQUENCE_ID_SHIFT) \|
	337	+ (EVENT_TYPE_MASK << EVENT_TYPE_SHIFT));
	338	+
	339	+ __skb_queue_head_init(&txq_list);
	340	+
	341	+ spin_lock_irqsave(&cpts->txq.lock, flags);
	342	+ skb_queue_splice_init(&cpts->txq, &txq_list);
	343	+ spin_unlock_irqrestore(&cpts->txq.lock, flags);
	344	+
	345	+ skb_queue_walk_safe(&txq_list, skb, tmp) {
	346	+ struct skb_shared_hwtstamps ssh;
	347	+ struct cpts_skb_cb_data *skb_cb =
	348	+ (struct cpts_skb_cb_data *)skb->cb;
	349	+
	350	+ if (mtype_seqid == skb_cb->skb_mtype_seqid) {
	351	+ memset(&ssh, 0, sizeof(ssh));
	352	+ ssh.hwtstamp = ns_to_ktime(event->timestamp);
	353	+ skb_tstamp_tx(skb, &ssh);
	354	+ found = true;
	355	+ __skb_unlink(skb, &txq_list);
	356	+ dev_consume_skb_any(skb);
	357	+ dev_dbg(cpts->dev, "match tx timestamp mtype_seqid %08x\n",
	358	+ mtype_seqid);
	359	+ break;
	360	+ }
	361	+
	362	+ if (time_after(jiffies, skb_cb->tmo)) {
	363	+ /* timeout any expired skbs over 1s */
	364	+ dev_dbg(cpts->dev, "expiring tx timestamp from txq\n");
	365	+ __skb_unlink(skb, &txq_list);
	366	+ dev_consume_skb_any(skb);
	367	+ }
	368	+ }
	369	+
	370	+ spin_lock_irqsave(&cpts->txq.lock, flags);
	371	+ skb_queue_splice(&txq_list, &cpts->txq);
	372	+ spin_unlock_irqrestore(&cpts->txq.lock, flags);
	373	+
	374	+ return found;
	375	+}
	376	+
	377	+static void cpts_process_events(struct cpts *cpts)
	378	+{
	379	+ struct list_head this, next;
	380	+ struct cpts_event *event;
	381	+ LIST_HEAD(events_free);
	382	+ unsigned long flags;
	383	+ LIST_HEAD(events);
	384	+
	385	+ spin_lock_irqsave(&cpts->lock, flags);
	386	+ list_splice_init(&cpts->events, &events);
	387	+ spin_unlock_irqrestore(&cpts->lock, flags);
	388	+
	389	+ list_for_each_safe(this, next, &events) {
	390	+ event = list_entry(this, struct cpts_event, list);
	391	+ if (cpts_match_tx_ts(cpts, event) \|\|
	392	+ time_after(jiffies, event->tmo)) {
	393	+ list_del_init(&event->list);
	394	+ list_add(&event->list, &events_free);
	395	+ }
	396	+ }
	397	+
	398	+ spin_lock_irqsave(&cpts->lock, flags);
	399	+ list_splice_tail(&events, &cpts->events);
	400	+ list_splice_tail(&events_free, &cpts->pool);
	401	+ spin_unlock_irqrestore(&cpts->lock, flags);
283	402	}
284	403
285	404	static long cpts_overflow_check(struct ptp_clock_info *ptp)
286	405	{
287	406	struct cpts *cpts = container_of(ptp, struct cpts, info);
288	407	unsigned long delay = cpts->ov_check_period;
289		- struct timespec64 ts;
290	408	unsigned long flags;
	409	+ u64 ns;
291	410
292		- spin_lock_irqsave(&cpts->lock, flags);
293		- ts = ns_to_timespec64(timecounter_read(&cpts->tc));
	411	+ mutex_lock(&cpts->ptp_clk_mutex);
294	412
295		- if (!skb_queue_empty(&cpts->txq))
296		- delay = CPTS_SKB_TX_WORK_TIMEOUT;
297		- spin_unlock_irqrestore(&cpts->lock, flags);
	413	+ cpts_update_cur_time(cpts, -1, NULL);
	414	+ ns = timecounter_read(&cpts->tc);
298	415
299		- pr_debug("cpts overflow check at %lld.%09ld\n",
300		- (long long)ts.tv_sec, ts.tv_nsec);
	416	+ cpts_process_events(cpts);
	417	+
	418	+ spin_lock_irqsave(&cpts->txq.lock, flags);
	419	+ if (!skb_queue_empty(&cpts->txq)) {
	420	+ cpts_purge_txq(cpts);
	421	+ if (!skb_queue_empty(&cpts->txq))
	422	+ delay = CPTS_SKB_TX_WORK_TIMEOUT;
	423	+ }
	424	+ spin_unlock_irqrestore(&cpts->txq.lock, flags);
	425	+
	426	+ dev_dbg(cpts->dev, "cpts overflow check at %lld\n", ns);
	427	+ mutex_unlock(&cpts->ptp_clk_mutex);
301	428	return (long)delay;
302	429	}
303	430
..	..	@@ -310,64 +437,46 @@
310	437	.pps = 0,
311	438	.adjfreq = cpts_ptp_adjfreq,
312	439	.adjtime = cpts_ptp_adjtime,
313		- .gettime64 = cpts_ptp_gettime,
	440	+ .gettimex64 = cpts_ptp_gettimeex,
314	441	.settime64 = cpts_ptp_settime,
315	442	.enable = cpts_ptp_enable,
316	443	.do_aux_work = cpts_overflow_check,
317	444	};
318	445
319		-static int cpts_match(struct sk_buff *skb, unsigned int ptp_class,
320		- u16 ts_seqid, u8 ts_msgtype)
	446	+static int cpts_skb_get_mtype_seqid(struct sk_buff skb, u32 mtype_seqid)
321	447	{
322		- u16 *seqid;
323		- unsigned int offset = 0;
324		- u8 msgtype, data = skb->data;
	448	+ unsigned int ptp_class = ptp_classify_raw(skb);
	449	+ struct ptp_header *hdr;
	450	+ u8 msgtype;
	451	+ u16 seqid;
325	452
326		- if (ptp_class & PTP_CLASS_VLAN)
327		- offset += VLAN_HLEN;
328		-
329		- switch (ptp_class & PTP_CLASS_PMASK) {
330		- case PTP_CLASS_IPV4:
331		- offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
332		- break;
333		- case PTP_CLASS_IPV6:
334		- offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
335		- break;
336		- case PTP_CLASS_L2:
337		- offset += ETH_HLEN;
338		- break;
339		- default:
340		- return 0;
341		- }
342		-
343		- if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
	453	+ if (ptp_class == PTP_CLASS_NONE)
344	454	return 0;
345	455
346		- if (unlikely(ptp_class & PTP_CLASS_V1))
347		- msgtype = data + offset + OFF_PTP_CONTROL;
348		- else
349		- msgtype = data + offset;
	456	+ hdr = ptp_parse_header(skb, ptp_class);
	457	+ if (!hdr)
	458	+ return 0;
350	459
351		- seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
	460	+ msgtype = ptp_get_msgtype(hdr, ptp_class);
	461	+ seqid = ntohs(hdr->sequence_id);
352	462
353		- return (ts_msgtype == (msgtype & 0xf) && ts_seqid == ntohs(seqid));
	463	+ *mtype_seqid = (msgtype & MESSAGE_TYPE_MASK) << MESSAGE_TYPE_SHIFT;
	464	+ *mtype_seqid \|= (seqid & SEQUENCE_ID_MASK) << SEQUENCE_ID_SHIFT;
	465	+
	466	+ return 1;
354	467	}
355	468
356		-static u64 cpts_find_ts(struct cpts cpts, struct sk_buff skb, int ev_type)
	469	+static u64 cpts_find_ts(struct cpts cpts, struct sk_buff skb,
	470	+ int ev_type, u32 skb_mtype_seqid)
357	471	{
358		- u64 ns = 0;
359		- struct cpts_event *event;
360	472	struct list_head this, next;
361		- unsigned int class = ptp_classify_raw(skb);
	473	+ struct cpts_event *event;
362	474	unsigned long flags;
363		- u16 seqid;
364		- u8 mtype;
	475	+ u32 mtype_seqid;
	476	+ u64 ns = 0;
365	477
366		- if (class == PTP_CLASS_NONE)
367		- return 0;
368		-
369		- spin_lock_irqsave(&cpts->lock, flags);
370	478	cpts_fifo_read(cpts, -1);
	479	+ spin_lock_irqsave(&cpts->lock, flags);
371	480	list_for_each_safe(this, next, &cpts->events) {
372	481	event = list_entry(this, struct cpts_event, list);
373	482	if (event_expired(event)) {
..	..	@@ -375,28 +484,18 @@
375	484	list_add(&event->list, &cpts->pool);
376	485	continue;
377	486	}
378		- mtype = (event->high >> MESSAGE_TYPE_SHIFT) & MESSAGE_TYPE_MASK;
379		- seqid = (event->high >> SEQUENCE_ID_SHIFT) & SEQUENCE_ID_MASK;
380		- if (ev_type == event_type(event) &&
381		- cpts_match(skb, class, seqid, mtype)) {
382		- ns = timecounter_cyc2time(&cpts->tc, event->low);
	487	+
	488	+ mtype_seqid = event->high &
	489	+ ((MESSAGE_TYPE_MASK << MESSAGE_TYPE_SHIFT) \|
	490	+ (SEQUENCE_ID_MASK << SEQUENCE_ID_SHIFT) \|
	491	+ (EVENT_TYPE_MASK << EVENT_TYPE_SHIFT));
	492	+
	493	+ if (mtype_seqid == skb_mtype_seqid) {
	494	+ ns = event->timestamp;
383	495	list_del_init(&event->list);
384	496	list_add(&event->list, &cpts->pool);
385	497	break;
386	498	}
387		- }
388		-
389		- if (ev_type == CPTS_EV_TX && !ns) {
390		- struct cpts_skb_cb_data *skb_cb =
391		- (struct cpts_skb_cb_data *)skb->cb;
392		- /* Not found, add frame to queue for processing later.
393		- * The periodic FIFO check will handle this.
394		- */
395		- skb_get(skb);
396		- /* get the timestamp for timeouts */
397		- skb_cb->tmo = jiffies + msecs_to_jiffies(100);
398		- __skb_queue_tail(&cpts->txq, skb);
399		- ptp_schedule_worker(cpts->clock, 0);
400	499	}
401	500	spin_unlock_irqrestore(&cpts->lock, flags);
402	501
..	..	@@ -405,12 +504,26 @@
405	504
406	505	void cpts_rx_timestamp(struct cpts cpts, struct sk_buff skb)
407	506	{
408		- u64 ns;
	507	+ struct cpts_skb_cb_data skb_cb = (struct cpts_skb_cb_data )skb->cb;
409	508	struct skb_shared_hwtstamps *ssh;
	509	+ int ret;
	510	+ u64 ns;
410	511
411		- if (!cpts->rx_enable)
	512	+ /* cpts_rx_timestamp() is called before eth_type_trans(), so
	513	+ * skb MAC Hdr properties are not configured yet. Hence need to
	514	+ * reset skb MAC header here
	515	+ */
	516	+ skb_reset_mac_header(skb);
	517	+ ret = cpts_skb_get_mtype_seqid(skb, &skb_cb->skb_mtype_seqid);
	518	+ if (!ret)
412	519	return;
413		- ns = cpts_find_ts(cpts, skb, CPTS_EV_RX);
	520	+
	521	+ skb_cb->skb_mtype_seqid \|= (CPTS_EV_RX << EVENT_TYPE_SHIFT);
	522	+
	523	+ dev_dbg(cpts->dev, "%s mtype seqid %08x\n",
	524	+ __func__, skb_cb->skb_mtype_seqid);
	525	+
	526	+ ns = cpts_find_ts(cpts, skb, CPTS_EV_RX, skb_cb->skb_mtype_seqid);
414	527	if (!ns)
415	528	return;
416	529	ssh = skb_hwtstamps(skb);
..	..	@@ -421,17 +534,27 @@
421	534
422	535	void cpts_tx_timestamp(struct cpts cpts, struct sk_buff skb)
423	536	{
424		- u64 ns;
425		- struct skb_shared_hwtstamps ssh;
	537	+ struct cpts_skb_cb_data skb_cb = (struct cpts_skb_cb_data )skb->cb;
	538	+ int ret;
426	539
427	540	if (!(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS))
428	541	return;
429		- ns = cpts_find_ts(cpts, skb, CPTS_EV_TX);
430		- if (!ns)
	542	+
	543	+ ret = cpts_skb_get_mtype_seqid(skb, &skb_cb->skb_mtype_seqid);
	544	+ if (!ret)
431	545	return;
432		- memset(&ssh, 0, sizeof(ssh));
433		- ssh.hwtstamp = ns_to_ktime(ns);
434		- skb_tstamp_tx(skb, &ssh);
	546	+
	547	+ skb_cb->skb_mtype_seqid \|= (CPTS_EV_TX << EVENT_TYPE_SHIFT);
	548	+
	549	+ dev_dbg(cpts->dev, "%s mtype seqid %08x\n",
	550	+ __func__, skb_cb->skb_mtype_seqid);
	551	+
	552	+ /* Always defer TX TS processing to PTP worker */
	553	+ skb_get(skb);
	554	+ /* get the timestamp for timeouts */
	555	+ skb_cb->tmo = jiffies + msecs_to_jiffies(CPTS_SKB_RX_TX_TMO);
	556	+ skb_queue_tail(&cpts->txq, skb);
	557	+ ptp_schedule_worker(cpts->clock, 0);
435	558	}
436	559	EXPORT_SYMBOL_GPL(cpts_tx_timestamp);
437	560
..	..	@@ -445,12 +568,14 @@
445	568	for (i = 0; i < CPTS_MAX_EVENTS; i++)
446	569	list_add(&cpts->pool_data[i].list, &cpts->pool);
447	570
448		- clk_enable(cpts->refclk);
	571	+ err = clk_enable(cpts->refclk);
	572	+ if (err)
	573	+ return err;
449	574
450	575	cpts_write32(cpts, CPTS_EN, control);
451	576	cpts_write32(cpts, TS_PEND_EN, int_enable);
452	577
453		- timecounter_init(&cpts->tc, &cpts->cc, ktime_to_ns(ktime_get_real()));
	578	+ timecounter_init(&cpts->tc, &cpts->cc, ktime_get_real_ns());
454	579
455	580	cpts->clock = ptp_clock_register(&cpts->info, cpts->dev);
456	581	if (IS_ERR(cpts->clock)) {
..	..	@@ -525,6 +650,82 @@
525	650	freq, cpts->cc.mult, cpts->cc.shift, (ns - NSEC_PER_SEC));
526	651	}
527	652
	653	+static int cpts_of_mux_clk_setup(struct cpts cpts, struct device_node node)
	654	+{
	655	+ struct device_node *refclk_np;
	656	+ const char **parent_names;
	657	+ unsigned int num_parents;
	658	+ struct clk_hw *clk_hw;
	659	+ int ret = -EINVAL;
	660	+ u32 *mux_table;
	661	+
	662	+ refclk_np = of_get_child_by_name(node, "cpts-refclk-mux");
	663	+ if (!refclk_np)
	664	+ /* refclk selection supported not for all SoCs */
	665	+ return 0;
	666	+
	667	+ num_parents = of_clk_get_parent_count(refclk_np);
	668	+ if (num_parents < 1) {
	669	+ dev_err(cpts->dev, "mux-clock %s must have parents\n",
	670	+ refclk_np->name);
	671	+ goto mux_fail;
	672	+ }
	673	+
	674	+ parent_names = devm_kzalloc(cpts->dev, (sizeof(char ) num_parents),
	675	+ GFP_KERNEL);
	676	+
	677	+ mux_table = devm_kzalloc(cpts->dev, sizeof(mux_table) num_parents,
	678	+ GFP_KERNEL);
	679	+ if (!mux_table \|\| !parent_names) {
	680	+ ret = -ENOMEM;
	681	+ goto mux_fail;
	682	+ }
	683	+
	684	+ of_clk_parent_fill(refclk_np, parent_names, num_parents);
	685	+
	686	+ ret = of_property_read_variable_u32_array(refclk_np, "ti,mux-tbl",
	687	+ mux_table,
	688	+ num_parents, num_parents);
	689	+ if (ret < 0)
	690	+ goto mux_fail;
	691	+
	692	+ clk_hw = clk_hw_register_mux_table(cpts->dev, refclk_np->name,
	693	+ parent_names, num_parents,
	694	+ 0,
	695	+ &cpts->reg->rftclk_sel, 0, 0x1F,
	696	+ 0, mux_table, NULL);
	697	+ if (IS_ERR(clk_hw)) {
	698	+ ret = PTR_ERR(clk_hw);
	699	+ goto mux_fail;
	700	+ }
	701	+
	702	+ ret = devm_add_action_or_reset(cpts->dev,
	703	+ (void()(void ))clk_hw_unregister_mux,
	704	+ clk_hw);
	705	+ if (ret) {
	706	+ dev_err(cpts->dev, "add clkmux unreg action %d", ret);
	707	+ goto mux_fail;
	708	+ }
	709	+
	710	+ ret = of_clk_add_hw_provider(refclk_np, of_clk_hw_simple_get, clk_hw);
	711	+ if (ret)
	712	+ goto mux_fail;
	713	+
	714	+ ret = devm_add_action_or_reset(cpts->dev,
	715	+ (void()(void ))of_clk_del_provider,
	716	+ refclk_np);
	717	+ if (ret) {
	718	+ dev_err(cpts->dev, "add clkmux provider unreg action %d", ret);
	719	+ goto mux_fail;
	720	+ }
	721	+
	722	+ return ret;
	723	+
	724	+mux_fail:
	725	+ of_node_put(refclk_np);
	726	+ return ret;
	727	+}
	728	+
528	729	static int cpts_of_parse(struct cpts cpts, struct device_node node)
529	730	{
530	731	int ret = -EINVAL;
..	..	@@ -540,7 +741,7 @@
540	741	(!cpts->cc.mult && cpts->cc.shift))
541	742	goto of_error;
542	743
543		- return 0;
	744	+ return cpts_of_mux_clk_setup(cpts, node);
544	745
545	746	of_error:
546	747	dev_err(cpts->dev, "CPTS: Missing property in the DT.\n");
..	..	@@ -548,7 +749,7 @@
548	749	}
549	750
550	751	struct cpts cpts_create(struct device dev, void __iomem *regs,
551		- struct device_node *node)
	752	+ struct device_node *node, u32 n_ext_ts)
552	753	{
553	754	struct cpts *cpts;
554	755	int ret;
..	..	@@ -559,15 +760,23 @@
559	760
560	761	cpts->dev = dev;
561	762	cpts->reg = (struct cpsw_cpts __iomem *)regs;
	763	+ cpts->irq_poll = true;
562	764	spin_lock_init(&cpts->lock);
	765	+ mutex_init(&cpts->ptp_clk_mutex);
	766	+ init_completion(&cpts->ts_push_complete);
563	767
564	768	ret = cpts_of_parse(cpts, node);
565	769	if (ret)
566	770	return ERR_PTR(ret);
567	771
568		- cpts->refclk = devm_clk_get(dev, "cpts");
	772	+ cpts->refclk = devm_get_clk_from_child(dev, node, "cpts");
	773	+ if (IS_ERR(cpts->refclk))
	774	+ /* try get clk from dev node for compatibility */
	775	+ cpts->refclk = devm_clk_get(dev, "cpts");
	776	+
569	777	if (IS_ERR(cpts->refclk)) {
570		- dev_err(dev, "Failed to get cpts refclk\n");
	778	+ dev_err(dev, "Failed to get cpts refclk %ld\n",
	779	+ PTR_ERR(cpts->refclk));
571	780	return ERR_CAST(cpts->refclk);
572	781	}
573	782
..	..	@@ -580,6 +789,9 @@
580	789	cpts->info = cpts_info;
581	790	cpts->phc_index = -1;
582	791
	792	+ if (n_ext_ts)
	793	+ cpts->info.n_ext_ts = n_ext_ts;
	794	+
583	795	cpts_calc_mult_shift(cpts);
584	796	/* save cc.mult original value as it can be modified
585	797	* by cpts_ptp_adjfreq().