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(ptp: 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(nr: HELLCREEK_HWTSTAMP_ENABLED, addr: &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(nr: HELLCREEK_HWTSTAMP_ENABLED, addr: &ps->state); |
108 | |
109 | return 0; |
110 | } |
111 | |
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(to: &config, from: ifr->ifr_data, n: sizeof(config))) |
123 | return -EFAULT; |
124 | |
125 | err = hellcreek_set_hwtstamp_config(hellcreek, port, config: &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(to: ifr->ifr_data, from: &config, n: sizeof(config)) ? |
133 | -EFAULT : 0; |
134 | } |
135 | |
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(to: ifr->ifr_data, from: config, n: 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 | */ |
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 | |
171 | static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr) |
172 | { |
173 | return be32_to_cpu(hdr->reserved2); |
174 | } |
175 | |
176 | static void hellcreek_clear_reserved_field(struct ptp_header *hdr) |
177 | { |
178 | hdr->reserved2 = 0; |
179 | } |
180 | |
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, offset: 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 */ |
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, offset: ts_reg); |
205 | nsh = hellcreek_ptp_read(hellcreek, offset: ts_reg); |
206 | nsh = hellcreek_ptp_read(hellcreek, offset: ts_reg); |
207 | nsh = hellcreek_ptp_read(hellcreek, offset: ts_reg); |
208 | nsl = hellcreek_ptp_read(hellcreek, offset: ts_reg); |
209 | |
210 | return (u64)nsl | ((u64)nsh << 16); |
211 | } |
212 | |
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, ts_reg: 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, ts_reg: data_reg); |
261 | ns += hellcreek_ptp_gettime_seconds(hellcreek, ns); |
262 | mutex_unlock(lock: &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(nr: HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, addr: &ps->state); |
278 | |
279 | /* Deliver a clone of the original outgoing tx_skb with tx hwtstamp */ |
280 | skb_complete_tx_timestamp(skb: tmp_skb, hwtstamps: &shhwtstamps); |
281 | |
282 | return 0; |
283 | |
284 | free_and_clear_skb: |
285 | dev_kfree_skb_any(skb: ps->tx_skb); |
286 | ps->tx_skb = NULL; |
287 | clear_bit_unlock(nr: HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, addr: &ps->state); |
288 | |
289 | return 0; |
290 | } |
291 | |
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(list: &received); |
303 | spin_lock_irqsave(&rxq->lock, flags); |
304 | skb_queue_splice_tail_init(list: rxq, head: &received); |
305 | spin_unlock_irqrestore(lock: &rxq->lock, flags); |
306 | |
307 | for (; skb; skb = __skb_dequeue(list: &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(lock: &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 | |
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(list: &ps->rx_queue); |
338 | if (skb) |
339 | hellcreek_get_rxts(hellcreek, ps, skb, rxq: &ps->rx_queue, port); |
340 | } |
341 | |
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, p: 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, port: i); |
358 | |
359 | hellcreek_rxtstamp_work(hellcreek, ps, port: i); |
360 | } |
361 | |
362 | return restart ? 1 : -1; |
363 | } |
364 | |
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(nr: HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, |
393 | addr: &ps->state)) { |
394 | kfree_skb(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(ptp: hellcreek->ptp_clock, delay: 0); |
406 | } |
407 | |
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(list: &ps->rx_queue, newsk: skb); |
434 | |
435 | ptp_schedule_worker(ptp: hellcreek->ptp_clock, delay: 0); |
436 | |
437 | return true; |
438 | } |
439 | |
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(list: &ps->rx_queue); |
446 | } |
447 | |
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, p: i)) |
456 | continue; |
457 | |
458 | hellcreek_hwtstamp_port_setup(hellcreek, port: 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 | |
471 | void hellcreek_hwtstamp_free(struct hellcreek *hellcreek) |
472 | { |
473 | /* Nothing todo */ |
474 | } |
475 | |