xref: /linux/drivers/net/dsa/hirschmann/hellcreek_hwtstamp.c (revision a1ff5a7d78a036d6c2178ee5acd6ba4946243800)
1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
2 /*
3  * DSA driver for:
4  * Hirschmann Hellcreek TSN switch.
5  *
6  * Copyright (C) 2019,2020 Hochschule Offenburg
7  * Copyright (C) 2019,2020 Linutronix GmbH
8  * Authors: Kamil Alkhouri <kamil.alkhouri@hs-offenburg.de>
9  *	    Kurt Kanzenbach <kurt@linutronix.de>
10  */
11 
12 #include <linux/ptp_classify.h>
13 
14 #include "hellcreek.h"
15 #include "hellcreek_hwtstamp.h"
16 #include "hellcreek_ptp.h"
17 
hellcreek_get_ts_info(struct dsa_switch * ds,int port,struct kernel_ethtool_ts_info * info)18 int hellcreek_get_ts_info(struct dsa_switch *ds, int port,
19 			  struct kernel_ethtool_ts_info *info)
20 {
21 	struct hellcreek *hellcreek = ds->priv;
22 
23 	info->phc_index = hellcreek->ptp_clock ?
24 		ptp_clock_index(hellcreek->ptp_clock) : -1;
25 	info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
26 		SOF_TIMESTAMPING_RX_HARDWARE |
27 		SOF_TIMESTAMPING_RAW_HARDWARE;
28 
29 	/* enabled tx timestamping */
30 	info->tx_types = BIT(HWTSTAMP_TX_ON);
31 
32 	/* L2 & L4 PTPv2 event rx messages are timestamped */
33 	info->rx_filters = BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);
34 
35 	return 0;
36 }
37 
38 /* Enabling/disabling TX and RX HW timestamping for different PTP messages is
39  * not available in the switch. Thus, this function only serves as a check if
40  * the user requested what is actually available or not
41  */
hellcreek_set_hwtstamp_config(struct hellcreek * hellcreek,int port,struct hwtstamp_config * config)42 static int hellcreek_set_hwtstamp_config(struct hellcreek *hellcreek, int port,
43 					 struct hwtstamp_config *config)
44 {
45 	struct hellcreek_port_hwtstamp *ps =
46 		&hellcreek->ports[port].port_hwtstamp;
47 	bool tx_tstamp_enable = false;
48 	bool rx_tstamp_enable = false;
49 
50 	/* Interaction with the timestamp hardware is prevented here.  It is
51 	 * enabled when this config function ends successfully
52 	 */
53 	clear_bit_unlock(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);
54 
55 	switch (config->tx_type) {
56 	case HWTSTAMP_TX_ON:
57 		tx_tstamp_enable = true;
58 		break;
59 
60 	/* TX HW timestamping can't be disabled on the switch */
61 	case HWTSTAMP_TX_OFF:
62 		config->tx_type = HWTSTAMP_TX_ON;
63 		break;
64 
65 	default:
66 		return -ERANGE;
67 	}
68 
69 	switch (config->rx_filter) {
70 	/* RX HW timestamping can't be disabled on the switch */
71 	case HWTSTAMP_FILTER_NONE:
72 		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
73 		break;
74 
75 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
76 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
77 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
78 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
79 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
80 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
81 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
82 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
83 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
84 		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
85 		rx_tstamp_enable = true;
86 		break;
87 
88 	/* RX HW timestamping can't be enabled for all messages on the switch */
89 	case HWTSTAMP_FILTER_ALL:
90 		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
91 		break;
92 
93 	default:
94 		return -ERANGE;
95 	}
96 
97 	if (!tx_tstamp_enable)
98 		return -ERANGE;
99 
100 	if (!rx_tstamp_enable)
101 		return -ERANGE;
102 
103 	/* If this point is reached, then the requested hwtstamp config is
104 	 * compatible with the hwtstamp offered by the switch.  Therefore,
105 	 * enable the interaction with the HW timestamping
106 	 */
107 	set_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);
108 
109 	return 0;
110 }
111 
hellcreek_port_hwtstamp_set(struct dsa_switch * ds,int port,struct ifreq * ifr)112 int hellcreek_port_hwtstamp_set(struct dsa_switch *ds, int port,
113 				struct ifreq *ifr)
114 {
115 	struct hellcreek *hellcreek = ds->priv;
116 	struct hellcreek_port_hwtstamp *ps;
117 	struct hwtstamp_config config;
118 	int err;
119 
120 	ps = &hellcreek->ports[port].port_hwtstamp;
121 
122 	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
123 		return -EFAULT;
124 
125 	err = hellcreek_set_hwtstamp_config(hellcreek, port, &config);
126 	if (err)
127 		return err;
128 
129 	/* Save the chosen configuration to be returned later */
130 	memcpy(&ps->tstamp_config, &config, sizeof(config));
131 
132 	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
133 		-EFAULT : 0;
134 }
135 
hellcreek_port_hwtstamp_get(struct dsa_switch * ds,int port,struct ifreq * ifr)136 int hellcreek_port_hwtstamp_get(struct dsa_switch *ds, int port,
137 				struct ifreq *ifr)
138 {
139 	struct hellcreek *hellcreek = ds->priv;
140 	struct hellcreek_port_hwtstamp *ps;
141 	struct hwtstamp_config *config;
142 
143 	ps = &hellcreek->ports[port].port_hwtstamp;
144 	config = &ps->tstamp_config;
145 
146 	return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
147 		-EFAULT : 0;
148 }
149 
150 /* Returns a pointer to the PTP header if the caller should time stamp, or NULL
151  * if the caller should not.
152  */
hellcreek_should_tstamp(struct hellcreek * hellcreek,int port,struct sk_buff * skb,unsigned int type)153 static struct ptp_header *hellcreek_should_tstamp(struct hellcreek *hellcreek,
154 						  int port, struct sk_buff *skb,
155 						  unsigned int type)
156 {
157 	struct hellcreek_port_hwtstamp *ps =
158 		&hellcreek->ports[port].port_hwtstamp;
159 	struct ptp_header *hdr;
160 
161 	hdr = ptp_parse_header(skb, type);
162 	if (!hdr)
163 		return NULL;
164 
165 	if (!test_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state))
166 		return NULL;
167 
168 	return hdr;
169 }
170 
hellcreek_get_reserved_field(const struct ptp_header * hdr)171 static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr)
172 {
173 	return be32_to_cpu(hdr->reserved2);
174 }
175 
hellcreek_clear_reserved_field(struct ptp_header * hdr)176 static void hellcreek_clear_reserved_field(struct ptp_header *hdr)
177 {
178 	hdr->reserved2 = 0;
179 }
180 
hellcreek_ptp_hwtstamp_available(struct hellcreek * hellcreek,unsigned int ts_reg)181 static int hellcreek_ptp_hwtstamp_available(struct hellcreek *hellcreek,
182 					    unsigned int ts_reg)
183 {
184 	u16 status;
185 
186 	status = hellcreek_ptp_read(hellcreek, ts_reg);
187 
188 	if (status & PR_TS_STATUS_TS_LOST)
189 		dev_err(hellcreek->dev,
190 			"Tx time stamp lost! This should never happen!\n");
191 
192 	/* If hwtstamp is not available, this means the previous hwtstamp was
193 	 * successfully read, and the one we need is not yet available
194 	 */
195 	return (status & PR_TS_STATUS_TS_AVAIL) ? 1 : 0;
196 }
197 
198 /* Get nanoseconds timestamp from timestamping unit */
hellcreek_ptp_hwtstamp_read(struct hellcreek * hellcreek,unsigned int ts_reg)199 static u64 hellcreek_ptp_hwtstamp_read(struct hellcreek *hellcreek,
200 				       unsigned int ts_reg)
201 {
202 	u16 nsl, nsh;
203 
204 	nsh = hellcreek_ptp_read(hellcreek, ts_reg);
205 	nsh = hellcreek_ptp_read(hellcreek, ts_reg);
206 	nsh = hellcreek_ptp_read(hellcreek, ts_reg);
207 	nsh = hellcreek_ptp_read(hellcreek, ts_reg);
208 	nsl = hellcreek_ptp_read(hellcreek, ts_reg);
209 
210 	return (u64)nsl | ((u64)nsh << 16);
211 }
212 
hellcreek_txtstamp_work(struct hellcreek * hellcreek,struct hellcreek_port_hwtstamp * ps,int port)213 static int hellcreek_txtstamp_work(struct hellcreek *hellcreek,
214 				   struct hellcreek_port_hwtstamp *ps, int port)
215 {
216 	struct skb_shared_hwtstamps shhwtstamps;
217 	unsigned int status_reg, data_reg;
218 	struct sk_buff *tmp_skb;
219 	int ts_status;
220 	u64 ns = 0;
221 
222 	if (!ps->tx_skb)
223 		return 0;
224 
225 	switch (port) {
226 	case 2:
227 		status_reg = PR_TS_TX_P1_STATUS_C;
228 		data_reg   = PR_TS_TX_P1_DATA_C;
229 		break;
230 	case 3:
231 		status_reg = PR_TS_TX_P2_STATUS_C;
232 		data_reg   = PR_TS_TX_P2_DATA_C;
233 		break;
234 	default:
235 		dev_err(hellcreek->dev, "Wrong port for timestamping!\n");
236 		return 0;
237 	}
238 
239 	ts_status = hellcreek_ptp_hwtstamp_available(hellcreek, status_reg);
240 
241 	/* Not available yet? */
242 	if (ts_status == 0) {
243 		/* Check whether the operation of reading the tx timestamp has
244 		 * exceeded its allowed period
245 		 */
246 		if (time_is_before_jiffies(ps->tx_tstamp_start +
247 					   TX_TSTAMP_TIMEOUT)) {
248 			dev_err(hellcreek->dev,
249 				"Timeout while waiting for Tx timestamp!\n");
250 			goto free_and_clear_skb;
251 		}
252 
253 		/* The timestamp should be available quickly, while getting it
254 		 * in high priority. Restart the work
255 		 */
256 		return 1;
257 	}
258 
259 	mutex_lock(&hellcreek->ptp_lock);
260 	ns  = hellcreek_ptp_hwtstamp_read(hellcreek, data_reg);
261 	ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
262 	mutex_unlock(&hellcreek->ptp_lock);
263 
264 	/* Now we have the timestamp in nanoseconds, store it in the correct
265 	 * structure in order to send it to the user
266 	 */
267 	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
268 	shhwtstamps.hwtstamp = ns_to_ktime(ns);
269 
270 	tmp_skb = ps->tx_skb;
271 	ps->tx_skb = NULL;
272 
273 	/* skb_complete_tx_timestamp() frees up the client to make another
274 	 * timestampable transmit.  We have to be ready for it by clearing the
275 	 * ps->tx_skb "flag" beforehand
276 	 */
277 	clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
278 
279 	/* Deliver a clone of the original outgoing tx_skb with tx hwtstamp */
280 	skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
281 
282 	return 0;
283 
284 free_and_clear_skb:
285 	dev_kfree_skb_any(ps->tx_skb);
286 	ps->tx_skb = NULL;
287 	clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
288 
289 	return 0;
290 }
291 
hellcreek_get_rxts(struct hellcreek * hellcreek,struct hellcreek_port_hwtstamp * ps,struct sk_buff * skb,struct sk_buff_head * rxq,int port)292 static void hellcreek_get_rxts(struct hellcreek *hellcreek,
293 			       struct hellcreek_port_hwtstamp *ps,
294 			       struct sk_buff *skb, struct sk_buff_head *rxq,
295 			       int port)
296 {
297 	struct skb_shared_hwtstamps *shwt;
298 	struct sk_buff_head received;
299 	unsigned long flags;
300 
301 	/* Construct Rx timestamps for all received PTP packets. */
302 	__skb_queue_head_init(&received);
303 	spin_lock_irqsave(&rxq->lock, flags);
304 	skb_queue_splice_tail_init(rxq, &received);
305 	spin_unlock_irqrestore(&rxq->lock, flags);
306 
307 	for (; skb; skb = __skb_dequeue(&received)) {
308 		struct ptp_header *hdr;
309 		unsigned int type;
310 		u64 ns;
311 
312 		/* Get nanoseconds from ptp packet */
313 		type = SKB_PTP_TYPE(skb);
314 		hdr  = ptp_parse_header(skb, type);
315 		ns   = hellcreek_get_reserved_field(hdr);
316 		hellcreek_clear_reserved_field(hdr);
317 
318 		/* Add seconds part */
319 		mutex_lock(&hellcreek->ptp_lock);
320 		ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
321 		mutex_unlock(&hellcreek->ptp_lock);
322 
323 		/* Save time stamp */
324 		shwt = skb_hwtstamps(skb);
325 		memset(shwt, 0, sizeof(*shwt));
326 		shwt->hwtstamp = ns_to_ktime(ns);
327 		netif_rx(skb);
328 	}
329 }
330 
hellcreek_rxtstamp_work(struct hellcreek * hellcreek,struct hellcreek_port_hwtstamp * ps,int port)331 static void hellcreek_rxtstamp_work(struct hellcreek *hellcreek,
332 				    struct hellcreek_port_hwtstamp *ps,
333 				    int port)
334 {
335 	struct sk_buff *skb;
336 
337 	skb = skb_dequeue(&ps->rx_queue);
338 	if (skb)
339 		hellcreek_get_rxts(hellcreek, ps, skb, &ps->rx_queue, port);
340 }
341 
hellcreek_hwtstamp_work(struct ptp_clock_info * ptp)342 long hellcreek_hwtstamp_work(struct ptp_clock_info *ptp)
343 {
344 	struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
345 	struct dsa_switch *ds = hellcreek->ds;
346 	int i, restart = 0;
347 
348 	for (i = 0; i < ds->num_ports; i++) {
349 		struct hellcreek_port_hwtstamp *ps;
350 
351 		if (!dsa_is_user_port(ds, i))
352 			continue;
353 
354 		ps = &hellcreek->ports[i].port_hwtstamp;
355 
356 		if (test_bit(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
357 			restart |= hellcreek_txtstamp_work(hellcreek, ps, i);
358 
359 		hellcreek_rxtstamp_work(hellcreek, ps, i);
360 	}
361 
362 	return restart ? 1 : -1;
363 }
364 
hellcreek_port_txtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb)365 void hellcreek_port_txtstamp(struct dsa_switch *ds, int port,
366 			     struct sk_buff *skb)
367 {
368 	struct hellcreek *hellcreek = ds->priv;
369 	struct hellcreek_port_hwtstamp *ps;
370 	struct ptp_header *hdr;
371 	struct sk_buff *clone;
372 	unsigned int type;
373 
374 	ps = &hellcreek->ports[port].port_hwtstamp;
375 
376 	type = ptp_classify_raw(skb);
377 	if (type == PTP_CLASS_NONE)
378 		return;
379 
380 	/* Make sure the message is a PTP message that needs to be timestamped
381 	 * and the interaction with the HW timestamping is enabled. If not, stop
382 	 * here
383 	 */
384 	hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
385 	if (!hdr)
386 		return;
387 
388 	clone = skb_clone_sk(skb);
389 	if (!clone)
390 		return;
391 
392 	if (test_and_set_bit_lock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS,
393 				  &ps->state)) {
394 		kfree_skb(clone);
395 		return;
396 	}
397 
398 	ps->tx_skb = clone;
399 
400 	/* store the number of ticks occurred since system start-up till this
401 	 * moment
402 	 */
403 	ps->tx_tstamp_start = jiffies;
404 
405 	ptp_schedule_worker(hellcreek->ptp_clock, 0);
406 }
407 
hellcreek_port_rxtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb,unsigned int type)408 bool hellcreek_port_rxtstamp(struct dsa_switch *ds, int port,
409 			     struct sk_buff *skb, unsigned int type)
410 {
411 	struct hellcreek *hellcreek = ds->priv;
412 	struct hellcreek_port_hwtstamp *ps;
413 	struct ptp_header *hdr;
414 
415 	ps = &hellcreek->ports[port].port_hwtstamp;
416 
417 	/* This check only fails if the user did not initialize hardware
418 	 * timestamping beforehand.
419 	 */
420 	if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
421 		return false;
422 
423 	/* Make sure the message is a PTP message that needs to be timestamped
424 	 * and the interaction with the HW timestamping is enabled. If not, stop
425 	 * here
426 	 */
427 	hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
428 	if (!hdr)
429 		return false;
430 
431 	SKB_PTP_TYPE(skb) = type;
432 
433 	skb_queue_tail(&ps->rx_queue, skb);
434 
435 	ptp_schedule_worker(hellcreek->ptp_clock, 0);
436 
437 	return true;
438 }
439 
hellcreek_hwtstamp_port_setup(struct hellcreek * hellcreek,int port)440 static void hellcreek_hwtstamp_port_setup(struct hellcreek *hellcreek, int port)
441 {
442 	struct hellcreek_port_hwtstamp *ps =
443 		&hellcreek->ports[port].port_hwtstamp;
444 
445 	skb_queue_head_init(&ps->rx_queue);
446 }
447 
hellcreek_hwtstamp_setup(struct hellcreek * hellcreek)448 int hellcreek_hwtstamp_setup(struct hellcreek *hellcreek)
449 {
450 	struct dsa_switch *ds = hellcreek->ds;
451 	int i;
452 
453 	/* Initialize timestamping ports. */
454 	for (i = 0; i < ds->num_ports; ++i) {
455 		if (!dsa_is_user_port(ds, i))
456 			continue;
457 
458 		hellcreek_hwtstamp_port_setup(hellcreek, i);
459 	}
460 
461 	/* Select the synchronized clock as the source timekeeper for the
462 	 * timestamps and enable inline timestamping.
463 	 */
464 	hellcreek_ptp_write(hellcreek, PR_SETTINGS_C_TS_SRC_TK_MASK |
465 			    PR_SETTINGS_C_RES3TS,
466 			    PR_SETTINGS_C);
467 
468 	return 0;
469 }
470 
hellcreek_hwtstamp_free(struct hellcreek * hellcreek)471 void hellcreek_hwtstamp_free(struct hellcreek *hellcreek)
472 {
473 	/* Nothing todo */
474 }
475