]>
git.proxmox.com Git - mirror_ubuntu-bionic-kernel.git/blob - drivers/net/ethernet/ti/cpts.c
2 * TI Common Platform Time Sync
4 * Copyright (C) 2012 Richard Cochran <richardcochran@gmail.com>
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.
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.
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
20 #include <linux/err.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>
34 #define cpts_read32(c, r) __raw_readl(&c->reg->r)
35 #define cpts_write32(c, v, r) __raw_writel(v, &c->reg->r)
37 static int event_expired(struct cpts_event
*event
)
39 return time_after(jiffies
, event
->tmo
);
42 static int event_type(struct cpts_event
*event
)
44 return (event
->high
>> EVENT_TYPE_SHIFT
) & EVENT_TYPE_MASK
;
47 static int cpts_fifo_pop(struct cpts
*cpts
, u32
*high
, u32
*low
)
49 u32 r
= cpts_read32(cpts
, intstat_raw
);
51 if (r
& TS_PEND_RAW
) {
52 *high
= cpts_read32(cpts
, event_high
);
53 *low
= cpts_read32(cpts
, event_low
);
54 cpts_write32(cpts
, EVENT_POP
, event_pop
);
61 * Returns zero if matching event type was found.
63 static int cpts_fifo_read(struct cpts
*cpts
, int match
)
67 struct cpts_event
*event
;
69 for (i
= 0; i
< CPTS_FIFO_DEPTH
; i
++) {
70 if (cpts_fifo_pop(cpts
, &hi
, &lo
))
72 if (list_empty(&cpts
->pool
)) {
73 pr_err("cpts: event pool is empty\n");
76 event
= list_first_entry(&cpts
->pool
, struct cpts_event
, list
);
77 event
->tmo
= jiffies
+ 2;
80 type
= event_type(event
);
85 list_del_init(&event
->list
);
86 list_add_tail(&event
->list
, &cpts
->events
);
93 pr_err("cpts: unknown event type\n");
99 return type
== match
? 0 : -1;
102 static cycle_t
cpts_systim_read(const struct cyclecounter
*cc
)
105 struct cpts_event
*event
;
106 struct list_head
*this, *next
;
107 struct cpts
*cpts
= container_of(cc
, struct cpts
, cc
);
109 cpts_write32(cpts
, TS_PUSH
, ts_push
);
110 if (cpts_fifo_read(cpts
, CPTS_EV_PUSH
))
111 pr_err("cpts: unable to obtain a time stamp\n");
113 list_for_each_safe(this, next
, &cpts
->events
) {
114 event
= list_entry(this, struct cpts_event
, list
);
115 if (event_type(event
) == CPTS_EV_PUSH
) {
116 list_del_init(&event
->list
);
117 list_add(&event
->list
, &cpts
->pool
);
126 /* PTP clock operations */
128 static int cpts_ptp_adjfreq(struct ptp_clock_info
*ptp
, s32 ppb
)
134 struct cpts
*cpts
= container_of(ptp
, struct cpts
, info
);
140 mult
= cpts
->cc_mult
;
143 diff
= div_u64(adj
, 1000000000ULL);
145 spin_lock_irqsave(&cpts
->lock
, flags
);
147 timecounter_read(&cpts
->tc
);
149 cpts
->cc
.mult
= neg_adj
? mult
- diff
: mult
+ diff
;
151 spin_unlock_irqrestore(&cpts
->lock
, flags
);
156 static int cpts_ptp_adjtime(struct ptp_clock_info
*ptp
, s64 delta
)
160 struct cpts
*cpts
= container_of(ptp
, struct cpts
, info
);
162 spin_lock_irqsave(&cpts
->lock
, flags
);
163 now
= timecounter_read(&cpts
->tc
);
165 timecounter_init(&cpts
->tc
, &cpts
->cc
, now
);
166 spin_unlock_irqrestore(&cpts
->lock
, flags
);
171 static int cpts_ptp_gettime(struct ptp_clock_info
*ptp
, struct timespec
*ts
)
176 struct cpts
*cpts
= container_of(ptp
, struct cpts
, info
);
178 spin_lock_irqsave(&cpts
->lock
, flags
);
179 ns
= timecounter_read(&cpts
->tc
);
180 spin_unlock_irqrestore(&cpts
->lock
, flags
);
182 ts
->tv_sec
= div_u64_rem(ns
, 1000000000, &remainder
);
183 ts
->tv_nsec
= remainder
;
188 static int cpts_ptp_settime(struct ptp_clock_info
*ptp
,
189 const struct timespec
*ts
)
193 struct cpts
*cpts
= container_of(ptp
, struct cpts
, info
);
195 ns
= ts
->tv_sec
* 1000000000ULL;
198 spin_lock_irqsave(&cpts
->lock
, flags
);
199 timecounter_init(&cpts
->tc
, &cpts
->cc
, ns
);
200 spin_unlock_irqrestore(&cpts
->lock
, flags
);
205 static int cpts_ptp_enable(struct ptp_clock_info
*ptp
,
206 struct ptp_clock_request
*rq
, int on
)
211 static struct ptp_clock_info cpts_info
= {
212 .owner
= THIS_MODULE
,
213 .name
= "CTPS timer",
218 .adjfreq
= cpts_ptp_adjfreq
,
219 .adjtime
= cpts_ptp_adjtime
,
220 .gettime
= cpts_ptp_gettime
,
221 .settime
= cpts_ptp_settime
,
222 .enable
= cpts_ptp_enable
,
225 static void cpts_overflow_check(struct work_struct
*work
)
228 struct cpts
*cpts
= container_of(work
, struct cpts
, overflow_work
.work
);
230 cpts_write32(cpts
, CPTS_EN
, control
);
231 cpts_write32(cpts
, TS_PEND_EN
, int_enable
);
232 cpts_ptp_gettime(&cpts
->info
, &ts
);
233 pr_debug("cpts overflow check at %ld.%09lu\n", ts
.tv_sec
, ts
.tv_nsec
);
234 schedule_delayed_work(&cpts
->overflow_work
, CPTS_OVERFLOW_PERIOD
);
237 #define CPTS_REF_CLOCK_NAME "cpsw_cpts_rft_clk"
239 static void cpts_clk_init(struct cpts
*cpts
)
241 cpts
->refclk
= clk_get(NULL
, CPTS_REF_CLOCK_NAME
);
242 if (IS_ERR(cpts
->refclk
)) {
243 pr_err("Failed to clk_get %s\n", CPTS_REF_CLOCK_NAME
);
247 clk_prepare_enable(cpts
->refclk
);
250 static void cpts_clk_release(struct cpts
*cpts
)
252 clk_disable(cpts
->refclk
);
253 clk_put(cpts
->refclk
);
256 static int cpts_match(struct sk_buff
*skb
, unsigned int ptp_class
,
257 u16 ts_seqid
, u8 ts_msgtype
)
261 u8
*msgtype
, *data
= skb
->data
;
264 case PTP_CLASS_V1_IPV4
:
265 case PTP_CLASS_V2_IPV4
:
266 offset
= ETH_HLEN
+ IPV4_HLEN(data
) + UDP_HLEN
;
268 case PTP_CLASS_V1_IPV6
:
269 case PTP_CLASS_V2_IPV6
:
272 case PTP_CLASS_V2_L2
:
275 case PTP_CLASS_V2_VLAN
:
276 offset
= ETH_HLEN
+ VLAN_HLEN
;
282 if (skb
->len
+ ETH_HLEN
< offset
+ OFF_PTP_SEQUENCE_ID
+ sizeof(*seqid
))
285 if (unlikely(ptp_class
& PTP_CLASS_V1
))
286 msgtype
= data
+ offset
+ OFF_PTP_CONTROL
;
288 msgtype
= data
+ offset
;
290 seqid
= (u16
*)(data
+ offset
+ OFF_PTP_SEQUENCE_ID
);
292 return (ts_msgtype
== (*msgtype
& 0xf) && ts_seqid
== ntohs(*seqid
));
295 static u64
cpts_find_ts(struct cpts
*cpts
, struct sk_buff
*skb
, int ev_type
)
298 struct cpts_event
*event
;
299 struct list_head
*this, *next
;
300 unsigned int class = ptp_classify_raw(skb
);
305 if (class == PTP_CLASS_NONE
)
308 spin_lock_irqsave(&cpts
->lock
, flags
);
309 cpts_fifo_read(cpts
, CPTS_EV_PUSH
);
310 list_for_each_safe(this, next
, &cpts
->events
) {
311 event
= list_entry(this, struct cpts_event
, list
);
312 if (event_expired(event
)) {
313 list_del_init(&event
->list
);
314 list_add(&event
->list
, &cpts
->pool
);
317 mtype
= (event
->high
>> MESSAGE_TYPE_SHIFT
) & MESSAGE_TYPE_MASK
;
318 seqid
= (event
->high
>> SEQUENCE_ID_SHIFT
) & SEQUENCE_ID_MASK
;
319 if (ev_type
== event_type(event
) &&
320 cpts_match(skb
, class, seqid
, mtype
)) {
321 ns
= timecounter_cyc2time(&cpts
->tc
, event
->low
);
322 list_del_init(&event
->list
);
323 list_add(&event
->list
, &cpts
->pool
);
327 spin_unlock_irqrestore(&cpts
->lock
, flags
);
332 void cpts_rx_timestamp(struct cpts
*cpts
, struct sk_buff
*skb
)
335 struct skb_shared_hwtstamps
*ssh
;
337 if (!cpts
->rx_enable
)
339 ns
= cpts_find_ts(cpts
, skb
, CPTS_EV_RX
);
342 ssh
= skb_hwtstamps(skb
);
343 memset(ssh
, 0, sizeof(*ssh
));
344 ssh
->hwtstamp
= ns_to_ktime(ns
);
347 void cpts_tx_timestamp(struct cpts
*cpts
, struct sk_buff
*skb
)
350 struct skb_shared_hwtstamps ssh
;
352 if (!(skb_shinfo(skb
)->tx_flags
& SKBTX_IN_PROGRESS
))
354 ns
= cpts_find_ts(cpts
, skb
, CPTS_EV_TX
);
357 memset(&ssh
, 0, sizeof(ssh
));
358 ssh
.hwtstamp
= ns_to_ktime(ns
);
359 skb_tstamp_tx(skb
, &ssh
);
362 #endif /*CONFIG_TI_CPTS*/
364 int cpts_register(struct device
*dev
, struct cpts
*cpts
,
367 #ifdef CONFIG_TI_CPTS
371 cpts
->info
= cpts_info
;
372 cpts
->clock
= ptp_clock_register(&cpts
->info
, dev
);
373 if (IS_ERR(cpts
->clock
)) {
374 err
= PTR_ERR(cpts
->clock
);
378 spin_lock_init(&cpts
->lock
);
380 cpts
->cc
.read
= cpts_systim_read
;
381 cpts
->cc
.mask
= CLOCKSOURCE_MASK(32);
382 cpts
->cc_mult
= mult
;
383 cpts
->cc
.mult
= mult
;
384 cpts
->cc
.shift
= shift
;
386 INIT_LIST_HEAD(&cpts
->events
);
387 INIT_LIST_HEAD(&cpts
->pool
);
388 for (i
= 0; i
< CPTS_MAX_EVENTS
; i
++)
389 list_add(&cpts
->pool_data
[i
].list
, &cpts
->pool
);
392 cpts_write32(cpts
, CPTS_EN
, control
);
393 cpts_write32(cpts
, TS_PEND_EN
, int_enable
);
395 spin_lock_irqsave(&cpts
->lock
, flags
);
396 timecounter_init(&cpts
->tc
, &cpts
->cc
, ktime_to_ns(ktime_get_real()));
397 spin_unlock_irqrestore(&cpts
->lock
, flags
);
399 INIT_DELAYED_WORK(&cpts
->overflow_work
, cpts_overflow_check
);
400 schedule_delayed_work(&cpts
->overflow_work
, CPTS_OVERFLOW_PERIOD
);
402 cpts
->phc_index
= ptp_clock_index(cpts
->clock
);
407 void cpts_unregister(struct cpts
*cpts
)
409 #ifdef CONFIG_TI_CPTS
411 ptp_clock_unregister(cpts
->clock
);
412 cancel_delayed_work_sync(&cpts
->overflow_work
);
415 cpts_clk_release(cpts
);