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(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(delta, 1000000000ULL, &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->ioaddr, priv->plat->est,
76 				     priv->plat->clk_ptp_rate);
77 		mutex_unlock(&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 	/* Caculate 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, &current_time);
92 		current_time_ns = timespec64_to_ktime(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(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(basetime,
99 						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->ioaddr, priv->plat->est,
106 					   priv->plat->clk_ptp_rate);
107 		mutex_unlock(&priv->plat->est->lock);
108 		if (ret)
109 			netdev_err(priv->dev, "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(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 		priv->plat->ext_snapshot_en = on;
196 		mutex_lock(&priv->aux_ts_lock);
197 		acr_value = readl(ptpaddr + PTP_ACR);
198 		acr_value &= ~PTP_ACR_MASK;
199 		if (on) {
200 			/* Enable External snapshot trigger */
201 			acr_value |= priv->plat->ext_snapshot_num;
202 			acr_value |= PTP_ACR_ATSFC;
203 			netdev_dbg(priv->dev, "Auxiliary Snapshot %d enabled.\n",
204 				   priv->plat->ext_snapshot_num >>
205 				   PTP_ACR_ATSEN_SHIFT);
206 		} else {
207 			netdev_dbg(priv->dev, "Auxiliary Snapshot %d disabled.\n",
208 				   priv->plat->ext_snapshot_num >>
209 				   PTP_ACR_ATSEN_SHIFT);
210 		}
211 		writel(acr_value, ptpaddr + PTP_ACR);
212 		mutex_unlock(&priv->aux_ts_lock);
213 		ret = 0;
214 		break;
215 
216 	default:
217 		break;
218 	}
219 
220 	return ret;
221 }
222 
223 /**
224  * stmmac_get_syncdevicetime
225  * @device: current device time
226  * @system: system counter value read synchronously with device time
227  * @ctx: context provided by timekeeping code
228  * Description: Read device and system clock simultaneously and return the
229  * corrected clock values in ns.
230  **/
231 static int stmmac_get_syncdevicetime(ktime_t *device,
232 				     struct system_counterval_t *system,
233 				     void *ctx)
234 {
235 	struct stmmac_priv *priv = (struct stmmac_priv *)ctx;
236 
237 	if (priv->plat->crosststamp)
238 		return priv->plat->crosststamp(device, system, ctx);
239 	else
240 		return -EOPNOTSUPP;
241 }
242 
243 static int stmmac_getcrosststamp(struct ptp_clock_info *ptp,
244 				 struct system_device_crosststamp *xtstamp)
245 {
246 	struct stmmac_priv *priv =
247 		container_of(ptp, struct stmmac_priv, ptp_clock_ops);
248 
249 	return get_device_system_crosststamp(stmmac_get_syncdevicetime,
250 					     priv, NULL, xtstamp);
251 }
252 
253 /* structure describing a PTP hardware clock */
254 static struct ptp_clock_info stmmac_ptp_clock_ops = {
255 	.owner = THIS_MODULE,
256 	.name = "stmmac ptp",
257 	.max_adj = 62500000,
258 	.n_alarm = 0,
259 	.n_ext_ts = 0, /* will be overwritten in stmmac_ptp_register */
260 	.n_per_out = 0, /* will be overwritten in stmmac_ptp_register */
261 	.n_pins = 0,
262 	.pps = 0,
263 	.adjfine = stmmac_adjust_freq,
264 	.adjtime = stmmac_adjust_time,
265 	.gettime64 = stmmac_get_time,
266 	.settime64 = stmmac_set_time,
267 	.enable = stmmac_enable,
268 	.getcrosststamp = stmmac_getcrosststamp,
269 };
270 
271 /**
272  * stmmac_ptp_register
273  * @priv: driver private structure
274  * Description: this function will register the ptp clock driver
275  * to kernel. It also does some house keeping work.
276  */
277 void stmmac_ptp_register(struct stmmac_priv *priv)
278 {
279 	int i;
280 
281 	for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
282 		if (i >= STMMAC_PPS_MAX)
283 			break;
284 		priv->pps[i].available = true;
285 	}
286 
287 	if (priv->plat->ptp_max_adj)
288 		stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;
289 
290 	/* Calculate the clock domain crossing (CDC) error if necessary */
291 	priv->plat->cdc_error_adj = 0;
292 	if (priv->plat->has_gmac4 && priv->plat->clk_ptp_rate)
293 		priv->plat->cdc_error_adj = (2 * NSEC_PER_SEC) / priv->plat->clk_ptp_rate;
294 
295 	stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
296 	stmmac_ptp_clock_ops.n_ext_ts = priv->dma_cap.aux_snapshot_n;
297 
298 	rwlock_init(&priv->ptp_lock);
299 	mutex_init(&priv->aux_ts_lock);
300 	priv->ptp_clock_ops = stmmac_ptp_clock_ops;
301 
302 	priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
303 					     priv->device);
304 	if (IS_ERR(priv->ptp_clock)) {
305 		netdev_err(priv->dev, "ptp_clock_register failed\n");
306 		priv->ptp_clock = NULL;
307 	} else if (priv->ptp_clock)
308 		netdev_info(priv->dev, "registered PTP clock\n");
309 }
310 
311 /**
312  * stmmac_ptp_unregister
313  * @priv: driver private structure
314  * Description: this function will remove/unregister the ptp clock driver
315  * from the kernel.
316  */
317 void stmmac_ptp_unregister(struct stmmac_priv *priv)
318 {
319 	if (priv->ptp_clock) {
320 		ptp_clock_unregister(priv->ptp_clock);
321 		priv->ptp_clock = NULL;
322 		pr_debug("Removed PTP HW clock successfully on %s\n",
323 			 priv->dev->name);
324 	}
325 
326 	mutex_destroy(&priv->aux_ts_lock);
327 }
328