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