1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /******************************************************************************* |
3 | PTP 1588 clock using the STMMAC. |
4 | |
5 | Copyright (C) 2013 Vayavya Labs Pvt Ltd |
6 | |
7 | |
8 | Author: Rayagond Kokatanur <rayagond@vayavyalabs.com> |
9 | *******************************************************************************/ |
10 | #include "stmmac.h" |
11 | #include "stmmac_ptp.h" |
12 | #include "dwmac4.h" |
13 | |
14 | /** |
15 | * stmmac_adjust_freq |
16 | * |
17 | * @ptp: pointer to ptp_clock_info structure |
18 | * @scaled_ppm: desired period change in scaled parts per million |
19 | * |
20 | * Description: this function will adjust the frequency of hardware clock. |
21 | * |
22 | * Scaled parts per million is ppm with a 16-bit binary fractional field. |
23 | */ |
24 | static int stmmac_adjust_freq(struct ptp_clock_info *ptp, long scaled_ppm) |
25 | { |
26 | struct stmmac_priv *priv = |
27 | container_of(ptp, struct stmmac_priv, ptp_clock_ops); |
28 | unsigned long flags; |
29 | u32 addend; |
30 | |
31 | addend = adjust_by_scaled_ppm(base: priv->default_addend, scaled_ppm); |
32 | |
33 | write_lock_irqsave(&priv->ptp_lock, flags); |
34 | stmmac_config_addend(priv, priv->ptpaddr, addend); |
35 | write_unlock_irqrestore(&priv->ptp_lock, flags); |
36 | |
37 | return 0; |
38 | } |
39 | |
40 | /** |
41 | * stmmac_adjust_time |
42 | * |
43 | * @ptp: pointer to ptp_clock_info structure |
44 | * @delta: desired change in nanoseconds |
45 | * |
46 | * Description: this function will shift/adjust the hardware clock time. |
47 | */ |
48 | static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta) |
49 | { |
50 | struct stmmac_priv *priv = |
51 | container_of(ptp, struct stmmac_priv, ptp_clock_ops); |
52 | unsigned long flags; |
53 | u32 sec, nsec; |
54 | u32 quotient, reminder; |
55 | int neg_adj = 0; |
56 | bool xmac, est_rst = false; |
57 | int ret; |
58 | |
59 | xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac; |
60 | |
61 | if (delta < 0) { |
62 | neg_adj = 1; |
63 | delta = -delta; |
64 | } |
65 | |
66 | quotient = div_u64_rem(dividend: delta, divisor: 1000000000ULL, remainder: &reminder); |
67 | sec = quotient; |
68 | nsec = reminder; |
69 | |
70 | /* If EST is enabled, disabled it before adjust ptp time. */ |
71 | if (priv->plat->est && priv->plat->est->enable) { |
72 | est_rst = true; |
73 | mutex_lock(&priv->plat->est->lock); |
74 | priv->plat->est->enable = false; |
75 | stmmac_est_configure(priv, priv, priv->plat->est, |
76 | priv->plat->clk_ptp_rate); |
77 | mutex_unlock(lock: &priv->plat->est->lock); |
78 | } |
79 | |
80 | write_lock_irqsave(&priv->ptp_lock, flags); |
81 | stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac); |
82 | write_unlock_irqrestore(&priv->ptp_lock, flags); |
83 | |
84 | /* Calculate new basetime and re-configured EST after PTP time adjust. */ |
85 | if (est_rst) { |
86 | struct timespec64 current_time, time; |
87 | ktime_t current_time_ns, basetime; |
88 | u64 cycle_time; |
89 | |
90 | mutex_lock(&priv->plat->est->lock); |
91 | priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, ¤t_time); |
92 | current_time_ns = timespec64_to_ktime(ts: current_time); |
93 | time.tv_nsec = priv->plat->est->btr_reserve[0]; |
94 | time.tv_sec = priv->plat->est->btr_reserve[1]; |
95 | basetime = timespec64_to_ktime(ts: time); |
96 | cycle_time = (u64)priv->plat->est->ctr[1] * NSEC_PER_SEC + |
97 | priv->plat->est->ctr[0]; |
98 | time = stmmac_calc_tas_basetime(old_base_time: basetime, |
99 | current_time: current_time_ns, |
100 | cycle_time); |
101 | |
102 | priv->plat->est->btr[0] = (u32)time.tv_nsec; |
103 | priv->plat->est->btr[1] = (u32)time.tv_sec; |
104 | priv->plat->est->enable = true; |
105 | ret = stmmac_est_configure(priv, priv, priv->plat->est, |
106 | priv->plat->clk_ptp_rate); |
107 | mutex_unlock(lock: &priv->plat->est->lock); |
108 | if (ret) |
109 | netdev_err(dev: priv->dev, format: "failed to configure EST\n" ); |
110 | } |
111 | |
112 | return 0; |
113 | } |
114 | |
115 | /** |
116 | * stmmac_get_time |
117 | * |
118 | * @ptp: pointer to ptp_clock_info structure |
119 | * @ts: pointer to hold time/result |
120 | * |
121 | * Description: this function will read the current time from the |
122 | * hardware clock and store it in @ts. |
123 | */ |
124 | static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts) |
125 | { |
126 | struct stmmac_priv *priv = |
127 | container_of(ptp, struct stmmac_priv, ptp_clock_ops); |
128 | unsigned long flags; |
129 | u64 ns = 0; |
130 | |
131 | read_lock_irqsave(&priv->ptp_lock, flags); |
132 | stmmac_get_systime(priv, priv->ptpaddr, &ns); |
133 | read_unlock_irqrestore(&priv->ptp_lock, flags); |
134 | |
135 | *ts = ns_to_timespec64(nsec: ns); |
136 | |
137 | return 0; |
138 | } |
139 | |
140 | /** |
141 | * stmmac_set_time |
142 | * |
143 | * @ptp: pointer to ptp_clock_info structure |
144 | * @ts: time value to set |
145 | * |
146 | * Description: this function will set the current time on the |
147 | * hardware clock. |
148 | */ |
149 | static int stmmac_set_time(struct ptp_clock_info *ptp, |
150 | const struct timespec64 *ts) |
151 | { |
152 | struct stmmac_priv *priv = |
153 | container_of(ptp, struct stmmac_priv, ptp_clock_ops); |
154 | unsigned long flags; |
155 | |
156 | write_lock_irqsave(&priv->ptp_lock, flags); |
157 | stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec); |
158 | write_unlock_irqrestore(&priv->ptp_lock, flags); |
159 | |
160 | return 0; |
161 | } |
162 | |
163 | static int stmmac_enable(struct ptp_clock_info *ptp, |
164 | struct ptp_clock_request *rq, int on) |
165 | { |
166 | struct stmmac_priv *priv = |
167 | container_of(ptp, struct stmmac_priv, ptp_clock_ops); |
168 | void __iomem *ptpaddr = priv->ptpaddr; |
169 | struct stmmac_pps_cfg *cfg; |
170 | int ret = -EOPNOTSUPP; |
171 | unsigned long flags; |
172 | u32 acr_value; |
173 | |
174 | switch (rq->type) { |
175 | case PTP_CLK_REQ_PEROUT: |
176 | /* Reject requests with unsupported flags */ |
177 | if (rq->perout.flags) |
178 | return -EOPNOTSUPP; |
179 | |
180 | cfg = &priv->pps[rq->perout.index]; |
181 | |
182 | cfg->start.tv_sec = rq->perout.start.sec; |
183 | cfg->start.tv_nsec = rq->perout.start.nsec; |
184 | cfg->period.tv_sec = rq->perout.period.sec; |
185 | cfg->period.tv_nsec = rq->perout.period.nsec; |
186 | |
187 | write_lock_irqsave(&priv->ptp_lock, flags); |
188 | ret = stmmac_flex_pps_config(priv, priv->ioaddr, |
189 | rq->perout.index, cfg, on, |
190 | priv->sub_second_inc, |
191 | priv->systime_flags); |
192 | write_unlock_irqrestore(&priv->ptp_lock, flags); |
193 | break; |
194 | case PTP_CLK_REQ_EXTTS: { |
195 | u8 channel; |
196 | |
197 | mutex_lock(&priv->aux_ts_lock); |
198 | acr_value = readl(addr: ptpaddr + PTP_ACR); |
199 | channel = ilog2(FIELD_GET(PTP_ACR_MASK, acr_value)); |
200 | acr_value &= ~PTP_ACR_MASK; |
201 | |
202 | if (on) { |
203 | if (FIELD_GET(PTP_ACR_MASK, acr_value)) { |
204 | netdev_err(dev: priv->dev, |
205 | format: "Cannot enable auxiliary snapshot %d as auxiliary snapshot %d is already enabled" , |
206 | rq->extts.index, channel); |
207 | mutex_unlock(lock: &priv->aux_ts_lock); |
208 | return -EBUSY; |
209 | } |
210 | |
211 | priv->plat->flags |= STMMAC_FLAG_EXT_SNAPSHOT_EN; |
212 | |
213 | /* Enable External snapshot trigger */ |
214 | acr_value |= PTP_ACR_ATSEN(rq->extts.index); |
215 | acr_value |= PTP_ACR_ATSFC; |
216 | } else { |
217 | priv->plat->flags &= ~STMMAC_FLAG_EXT_SNAPSHOT_EN; |
218 | } |
219 | netdev_dbg(priv->dev, "Auxiliary Snapshot %d %s.\n" , |
220 | rq->extts.index, on ? "enabled" : "disabled" ); |
221 | writel(val: acr_value, addr: ptpaddr + PTP_ACR); |
222 | mutex_unlock(lock: &priv->aux_ts_lock); |
223 | /* wait for auxts fifo clear to finish */ |
224 | ret = readl_poll_timeout(ptpaddr + PTP_ACR, acr_value, |
225 | !(acr_value & PTP_ACR_ATSFC), |
226 | 10, 10000); |
227 | break; |
228 | } |
229 | |
230 | default: |
231 | break; |
232 | } |
233 | |
234 | return ret; |
235 | } |
236 | |
237 | /** |
238 | * stmmac_get_syncdevicetime |
239 | * @device: current device time |
240 | * @system: system counter value read synchronously with device time |
241 | * @ctx: context provided by timekeeping code |
242 | * Description: Read device and system clock simultaneously and return the |
243 | * corrected clock values in ns. |
244 | **/ |
245 | static int stmmac_get_syncdevicetime(ktime_t *device, |
246 | struct system_counterval_t *system, |
247 | void *ctx) |
248 | { |
249 | struct stmmac_priv *priv = (struct stmmac_priv *)ctx; |
250 | |
251 | if (priv->plat->crosststamp) |
252 | return priv->plat->crosststamp(device, system, ctx); |
253 | else |
254 | return -EOPNOTSUPP; |
255 | } |
256 | |
257 | static int stmmac_getcrosststamp(struct ptp_clock_info *ptp, |
258 | struct system_device_crosststamp *xtstamp) |
259 | { |
260 | struct stmmac_priv *priv = |
261 | container_of(ptp, struct stmmac_priv, ptp_clock_ops); |
262 | |
263 | return get_device_system_crosststamp(get_time_fn: stmmac_get_syncdevicetime, |
264 | ctx: priv, NULL, xtstamp); |
265 | } |
266 | |
267 | /* structure describing a PTP hardware clock */ |
268 | static struct ptp_clock_info stmmac_ptp_clock_ops = { |
269 | .owner = THIS_MODULE, |
270 | .name = "stmmac ptp" , |
271 | .max_adj = 62500000, |
272 | .n_alarm = 0, |
273 | .n_ext_ts = 0, /* will be overwritten in stmmac_ptp_register */ |
274 | .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */ |
275 | .n_pins = 0, |
276 | .pps = 0, |
277 | .adjfine = stmmac_adjust_freq, |
278 | .adjtime = stmmac_adjust_time, |
279 | .gettime64 = stmmac_get_time, |
280 | .settime64 = stmmac_set_time, |
281 | .enable = stmmac_enable, |
282 | .getcrosststamp = stmmac_getcrosststamp, |
283 | }; |
284 | |
285 | /** |
286 | * stmmac_ptp_register |
287 | * @priv: driver private structure |
288 | * Description: this function will register the ptp clock driver |
289 | * to kernel. It also does some house keeping work. |
290 | */ |
291 | void stmmac_ptp_register(struct stmmac_priv *priv) |
292 | { |
293 | int i; |
294 | |
295 | for (i = 0; i < priv->dma_cap.pps_out_num; i++) { |
296 | if (i >= STMMAC_PPS_MAX) |
297 | break; |
298 | priv->pps[i].available = true; |
299 | } |
300 | |
301 | if (priv->plat->ptp_max_adj) |
302 | stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj; |
303 | |
304 | /* Calculate the clock domain crossing (CDC) error if necessary */ |
305 | priv->plat->cdc_error_adj = 0; |
306 | if (priv->plat->has_gmac4 && priv->plat->clk_ptp_rate) |
307 | priv->plat->cdc_error_adj = (2 * NSEC_PER_SEC) / priv->plat->clk_ptp_rate; |
308 | |
309 | stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num; |
310 | stmmac_ptp_clock_ops.n_ext_ts = priv->dma_cap.aux_snapshot_n; |
311 | |
312 | rwlock_init(&priv->ptp_lock); |
313 | mutex_init(&priv->aux_ts_lock); |
314 | priv->ptp_clock_ops = stmmac_ptp_clock_ops; |
315 | |
316 | priv->ptp_clock = ptp_clock_register(info: &priv->ptp_clock_ops, |
317 | parent: priv->device); |
318 | if (IS_ERR(ptr: priv->ptp_clock)) { |
319 | netdev_err(dev: priv->dev, format: "ptp_clock_register failed\n" ); |
320 | priv->ptp_clock = NULL; |
321 | } else if (priv->ptp_clock) |
322 | netdev_info(dev: priv->dev, format: "registered PTP clock\n" ); |
323 | } |
324 | |
325 | /** |
326 | * stmmac_ptp_unregister |
327 | * @priv: driver private structure |
328 | * Description: this function will remove/unregister the ptp clock driver |
329 | * from the kernel. |
330 | */ |
331 | void stmmac_ptp_unregister(struct stmmac_priv *priv) |
332 | { |
333 | if (priv->ptp_clock) { |
334 | ptp_clock_unregister(ptp: priv->ptp_clock); |
335 | priv->ptp_clock = NULL; |
336 | pr_debug("Removed PTP HW clock successfully on %s\n" , |
337 | priv->dev->name); |
338 | } |
339 | |
340 | mutex_destroy(lock: &priv->aux_ts_lock); |
341 | } |
342 | |