hellcreek_hwtstamp.c

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