Loading...
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}
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 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 /* 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
116int 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
140int 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 */
157static 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
175static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr)
176{
177 return be32_to_cpu(hdr->reserved2);
178}
179
180static void hellcreek_clear_reserved_field(struct ptp_header *hdr)
181{
182 hdr->reserved2 = 0;
183}
184
185static 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 */
203static 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
217static 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
288free_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
296static 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
342static 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
353long 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
376void hellcreek_port_txtstamp(struct dsa_switch *ds, int port,
377 struct sk_buff *skb)
378{
379 struct hellcreek *hellcreek = ds->priv;
380 struct hellcreek_port_hwtstamp *ps;
381 struct ptp_header *hdr;
382 struct sk_buff *clone;
383 unsigned int type;
384
385 ps = &hellcreek->ports[port].port_hwtstamp;
386
387 type = ptp_classify_raw(skb);
388 if (type == PTP_CLASS_NONE)
389 return;
390
391 /* Make sure the message is a PTP message that needs to be timestamped
392 * and the interaction with the HW timestamping is enabled. If not, stop
393 * here
394 */
395 hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
396 if (!hdr)
397 return;
398
399 clone = skb_clone_sk(skb);
400 if (!clone)
401 return;
402
403 if (test_and_set_bit_lock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS,
404 &ps->state)) {
405 kfree_skb(clone);
406 return;
407 }
408
409 ps->tx_skb = clone;
410
411 /* store the number of ticks occurred since system start-up till this
412 * moment
413 */
414 ps->tx_tstamp_start = jiffies;
415
416 ptp_schedule_worker(hellcreek->ptp_clock, 0);
417}
418
419bool hellcreek_port_rxtstamp(struct dsa_switch *ds, int port,
420 struct sk_buff *skb, unsigned int type)
421{
422 struct hellcreek *hellcreek = ds->priv;
423 struct hellcreek_port_hwtstamp *ps;
424 struct ptp_header *hdr;
425
426 ps = &hellcreek->ports[port].port_hwtstamp;
427
428 /* This check only fails if the user did not initialize hardware
429 * timestamping beforehand.
430 */
431 if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
432 return false;
433
434 /* Make sure the message is a PTP message that needs to be timestamped
435 * and the interaction with the HW timestamping is enabled. If not, stop
436 * here
437 */
438 hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
439 if (!hdr)
440 return false;
441
442 SKB_PTP_TYPE(skb) = type;
443
444 skb_queue_tail(&ps->rx_queue, skb);
445
446 ptp_schedule_worker(hellcreek->ptp_clock, 0);
447
448 return true;
449}
450
451static void hellcreek_hwtstamp_port_setup(struct hellcreek *hellcreek, int port)
452{
453 struct hellcreek_port_hwtstamp *ps =
454 &hellcreek->ports[port].port_hwtstamp;
455
456 skb_queue_head_init(&ps->rx_queue);
457}
458
459int hellcreek_hwtstamp_setup(struct hellcreek *hellcreek)
460{
461 struct dsa_switch *ds = hellcreek->ds;
462 int i;
463
464 /* Initialize timestamping ports. */
465 for (i = 0; i < ds->num_ports; ++i) {
466 if (!dsa_is_user_port(ds, i))
467 continue;
468
469 hellcreek_hwtstamp_port_setup(hellcreek, i);
470 }
471
472 /* Select the synchronized clock as the source timekeeper for the
473 * timestamps and enable inline timestamping.
474 */
475 hellcreek_ptp_write(hellcreek, PR_SETTINGS_C_TS_SRC_TK_MASK |
476 PR_SETTINGS_C_RES3TS,
477 PR_SETTINGS_C);
478
479 return 0;
480}
481
482void hellcreek_hwtstamp_free(struct hellcreek *hellcreek)
483{
484 /* Nothing todo */
485}