Loading...
Note: File does not exist in v4.17.
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 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 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
136int 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 */
153static 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
171static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr)
172{
173 return be32_to_cpu(hdr->reserved2);
174}
175
176static void hellcreek_clear_reserved_field(struct ptp_header *hdr)
177{
178 hdr->reserved2 = 0;
179}
180
181static 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 */
199static 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
213static 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
284free_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
292static 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
331static 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
342long 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
365void 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
408bool 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
440static 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
448int 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
471void hellcreek_hwtstamp_free(struct hellcreek *hellcreek)
472{
473 /* Nothing todo */
474}