1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Marvell 88E6xxx Switch hardware timestamping support |
4 | * |
5 | * Copyright (c) 2008 Marvell Semiconductor |
6 | * |
7 | * Copyright (c) 2017 National Instruments |
8 | * Erik Hons <erik.hons@ni.com> |
9 | * Brandon Streiff <brandon.streiff@ni.com> |
10 | * Dane Wagner <dane.wagner@ni.com> |
11 | */ |
12 | |
13 | #include "chip.h" |
14 | #include "global2.h" |
15 | #include "hwtstamp.h" |
16 | #include "ptp.h" |
17 | #include <linux/ptp_classify.h> |
18 | |
19 | #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb)) |
20 | |
21 | static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port, |
22 | int addr, u16 *data, int len) |
23 | { |
24 | if (!chip->info->ops->avb_ops->port_ptp_read) |
25 | return -EOPNOTSUPP; |
26 | |
27 | return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr, |
28 | data, len); |
29 | } |
30 | |
31 | static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port, |
32 | int addr, u16 data) |
33 | { |
34 | if (!chip->info->ops->avb_ops->port_ptp_write) |
35 | return -EOPNOTSUPP; |
36 | |
37 | return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr, |
38 | data); |
39 | } |
40 | |
41 | static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr, |
42 | u16 data) |
43 | { |
44 | if (!chip->info->ops->avb_ops->ptp_write) |
45 | return -EOPNOTSUPP; |
46 | |
47 | return chip->info->ops->avb_ops->ptp_write(chip, addr, data); |
48 | } |
49 | |
50 | static int mv88e6xxx_ptp_read(struct mv88e6xxx_chip *chip, int addr, |
51 | u16 *data) |
52 | { |
53 | if (!chip->info->ops->avb_ops->ptp_read) |
54 | return -EOPNOTSUPP; |
55 | |
56 | return chip->info->ops->avb_ops->ptp_read(chip, addr, data, 1); |
57 | } |
58 | |
59 | /* TX_TSTAMP_TIMEOUT: This limits the time spent polling for a TX |
60 | * timestamp. When working properly, hardware will produce a timestamp |
61 | * within 1ms. Software may enounter delays due to MDIO contention, so |
62 | * the timeout is set accordingly. |
63 | */ |
64 | #define TX_TSTAMP_TIMEOUT msecs_to_jiffies(40) |
65 | |
66 | int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port, |
67 | struct ethtool_ts_info *info) |
68 | { |
69 | const struct mv88e6xxx_ptp_ops *ptp_ops; |
70 | struct mv88e6xxx_chip *chip; |
71 | |
72 | chip = ds->priv; |
73 | ptp_ops = chip->info->ops->ptp_ops; |
74 | |
75 | if (!chip->info->ptp_support) |
76 | return -EOPNOTSUPP; |
77 | |
78 | info->so_timestamping = |
79 | SOF_TIMESTAMPING_TX_HARDWARE | |
80 | SOF_TIMESTAMPING_RX_HARDWARE | |
81 | SOF_TIMESTAMPING_RAW_HARDWARE; |
82 | info->phc_index = ptp_clock_index(ptp: chip->ptp_clock); |
83 | info->tx_types = |
84 | (1 << HWTSTAMP_TX_OFF) | |
85 | (1 << HWTSTAMP_TX_ON); |
86 | info->rx_filters = ptp_ops->rx_filters; |
87 | |
88 | return 0; |
89 | } |
90 | |
91 | static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port, |
92 | struct hwtstamp_config *config) |
93 | { |
94 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
95 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; |
96 | bool tstamp_enable = false; |
97 | |
98 | /* Prevent the TX/RX paths from trying to interact with the |
99 | * timestamp hardware while we reconfigure it. |
100 | */ |
101 | clear_bit_unlock(nr: MV88E6XXX_HWTSTAMP_ENABLED, addr: &ps->state); |
102 | |
103 | switch (config->tx_type) { |
104 | case HWTSTAMP_TX_OFF: |
105 | tstamp_enable = false; |
106 | break; |
107 | case HWTSTAMP_TX_ON: |
108 | tstamp_enable = true; |
109 | break; |
110 | default: |
111 | return -ERANGE; |
112 | } |
113 | |
114 | /* The switch supports timestamping both L2 and L4; one cannot be |
115 | * disabled independently of the other. |
116 | */ |
117 | |
118 | if (!(BIT(config->rx_filter) & ptp_ops->rx_filters)) { |
119 | config->rx_filter = HWTSTAMP_FILTER_NONE; |
120 | dev_dbg(chip->dev, "Unsupported rx_filter %d\n" , |
121 | config->rx_filter); |
122 | return -ERANGE; |
123 | } |
124 | |
125 | switch (config->rx_filter) { |
126 | case HWTSTAMP_FILTER_NONE: |
127 | tstamp_enable = false; |
128 | break; |
129 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: |
130 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: |
131 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: |
132 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: |
133 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: |
134 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: |
135 | case HWTSTAMP_FILTER_PTP_V2_EVENT: |
136 | case HWTSTAMP_FILTER_PTP_V2_SYNC: |
137 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: |
138 | config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; |
139 | break; |
140 | case HWTSTAMP_FILTER_ALL: |
141 | default: |
142 | config->rx_filter = HWTSTAMP_FILTER_NONE; |
143 | return -ERANGE; |
144 | } |
145 | |
146 | mv88e6xxx_reg_lock(chip); |
147 | if (tstamp_enable) { |
148 | chip->enable_count += 1; |
149 | if (chip->enable_count == 1 && ptp_ops->global_enable) |
150 | ptp_ops->global_enable(chip); |
151 | if (ptp_ops->port_enable) |
152 | ptp_ops->port_enable(chip, port); |
153 | } else { |
154 | if (ptp_ops->port_disable) |
155 | ptp_ops->port_disable(chip, port); |
156 | chip->enable_count -= 1; |
157 | if (chip->enable_count == 0 && ptp_ops->global_disable) |
158 | ptp_ops->global_disable(chip); |
159 | } |
160 | mv88e6xxx_reg_unlock(chip); |
161 | |
162 | /* Once hardware has been configured, enable timestamp checks |
163 | * in the RX/TX paths. |
164 | */ |
165 | if (tstamp_enable) |
166 | set_bit(nr: MV88E6XXX_HWTSTAMP_ENABLED, addr: &ps->state); |
167 | |
168 | return 0; |
169 | } |
170 | |
171 | int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port, |
172 | struct ifreq *ifr) |
173 | { |
174 | struct mv88e6xxx_chip *chip = ds->priv; |
175 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; |
176 | struct hwtstamp_config config; |
177 | int err; |
178 | |
179 | if (!chip->info->ptp_support) |
180 | return -EOPNOTSUPP; |
181 | |
182 | if (copy_from_user(to: &config, from: ifr->ifr_data, n: sizeof(config))) |
183 | return -EFAULT; |
184 | |
185 | err = mv88e6xxx_set_hwtstamp_config(chip, port, config: &config); |
186 | if (err) |
187 | return err; |
188 | |
189 | /* Save the chosen configuration to be returned later. */ |
190 | memcpy(&ps->tstamp_config, &config, sizeof(config)); |
191 | |
192 | return copy_to_user(to: ifr->ifr_data, from: &config, n: sizeof(config)) ? |
193 | -EFAULT : 0; |
194 | } |
195 | |
196 | int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port, |
197 | struct ifreq *ifr) |
198 | { |
199 | struct mv88e6xxx_chip *chip = ds->priv; |
200 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; |
201 | struct hwtstamp_config *config = &ps->tstamp_config; |
202 | |
203 | if (!chip->info->ptp_support) |
204 | return -EOPNOTSUPP; |
205 | |
206 | return copy_to_user(to: ifr->ifr_data, from: config, n: sizeof(*config)) ? |
207 | -EFAULT : 0; |
208 | } |
209 | |
210 | /* Returns a pointer to the PTP header if the caller should time stamp, |
211 | * or NULL if the caller should not. |
212 | */ |
213 | static struct ptp_header *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, |
214 | int port, struct sk_buff *skb, |
215 | unsigned int type) |
216 | { |
217 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; |
218 | struct ptp_header *hdr; |
219 | |
220 | if (!chip->info->ptp_support) |
221 | return NULL; |
222 | |
223 | hdr = ptp_parse_header(skb, type); |
224 | if (!hdr) |
225 | return NULL; |
226 | |
227 | if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state)) |
228 | return NULL; |
229 | |
230 | return hdr; |
231 | } |
232 | |
233 | static int mv88e6xxx_ts_valid(u16 status) |
234 | { |
235 | if (!(status & MV88E6XXX_PTP_TS_VALID)) |
236 | return 0; |
237 | if (status & MV88E6XXX_PTP_TS_STATUS_MASK) |
238 | return 0; |
239 | return 1; |
240 | } |
241 | |
242 | static int seq_match(struct sk_buff *skb, u16 ts_seqid) |
243 | { |
244 | unsigned int type = SKB_PTP_TYPE(skb); |
245 | struct ptp_header *hdr; |
246 | |
247 | hdr = ptp_parse_header(skb, type); |
248 | |
249 | return ts_seqid == ntohs(hdr->sequence_id); |
250 | } |
251 | |
252 | static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip, |
253 | struct mv88e6xxx_port_hwtstamp *ps, |
254 | struct sk_buff *skb, u16 reg, |
255 | struct sk_buff_head *rxq) |
256 | { |
257 | u16 buf[4] = { 0 }, status, seq_id; |
258 | struct skb_shared_hwtstamps *shwt; |
259 | struct sk_buff_head received; |
260 | u64 ns, timelo, timehi; |
261 | unsigned long flags; |
262 | int err; |
263 | |
264 | /* The latched timestamp belongs to one of the received frames. */ |
265 | __skb_queue_head_init(list: &received); |
266 | spin_lock_irqsave(&rxq->lock, flags); |
267 | skb_queue_splice_tail_init(list: rxq, head: &received); |
268 | spin_unlock_irqrestore(lock: &rxq->lock, flags); |
269 | |
270 | mv88e6xxx_reg_lock(chip); |
271 | err = mv88e6xxx_port_ptp_read(chip, port: ps->port_id, |
272 | addr: reg, data: buf, ARRAY_SIZE(buf)); |
273 | mv88e6xxx_reg_unlock(chip); |
274 | if (err) |
275 | pr_err("failed to get the receive time stamp\n" ); |
276 | |
277 | status = buf[0]; |
278 | timelo = buf[1]; |
279 | timehi = buf[2]; |
280 | seq_id = buf[3]; |
281 | |
282 | if (status & MV88E6XXX_PTP_TS_VALID) { |
283 | mv88e6xxx_reg_lock(chip); |
284 | err = mv88e6xxx_port_ptp_write(chip, port: ps->port_id, addr: reg, data: 0); |
285 | mv88e6xxx_reg_unlock(chip); |
286 | if (err) |
287 | pr_err("failed to clear the receive status\n" ); |
288 | } |
289 | /* Since the device can only handle one time stamp at a time, |
290 | * we purge any extra frames from the queue. |
291 | */ |
292 | for ( ; skb; skb = __skb_dequeue(list: &received)) { |
293 | if (mv88e6xxx_ts_valid(status) && seq_match(skb, ts_seqid: seq_id)) { |
294 | ns = timehi << 16 | timelo; |
295 | |
296 | mv88e6xxx_reg_lock(chip); |
297 | ns = timecounter_cyc2time(tc: &chip->tstamp_tc, cycle_tstamp: ns); |
298 | mv88e6xxx_reg_unlock(chip); |
299 | shwt = skb_hwtstamps(skb); |
300 | memset(shwt, 0, sizeof(*shwt)); |
301 | shwt->hwtstamp = ns_to_ktime(ns); |
302 | status &= ~MV88E6XXX_PTP_TS_VALID; |
303 | } |
304 | netif_rx(skb); |
305 | } |
306 | } |
307 | |
308 | static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip, |
309 | struct mv88e6xxx_port_hwtstamp *ps) |
310 | { |
311 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
312 | struct sk_buff *skb; |
313 | |
314 | skb = skb_dequeue(list: &ps->rx_queue); |
315 | |
316 | if (skb) |
317 | mv88e6xxx_get_rxts(chip, ps, skb, reg: ptp_ops->arr0_sts_reg, |
318 | rxq: &ps->rx_queue); |
319 | |
320 | skb = skb_dequeue(list: &ps->rx_queue2); |
321 | if (skb) |
322 | mv88e6xxx_get_rxts(chip, ps, skb, reg: ptp_ops->arr1_sts_reg, |
323 | rxq: &ps->rx_queue2); |
324 | } |
325 | |
326 | static int is_pdelay_resp(const struct ptp_header *hdr) |
327 | { |
328 | return (hdr->tsmt & 0xf) == 3; |
329 | } |
330 | |
331 | bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port, |
332 | struct sk_buff *skb, unsigned int type) |
333 | { |
334 | struct mv88e6xxx_port_hwtstamp *ps; |
335 | struct mv88e6xxx_chip *chip; |
336 | struct ptp_header *hdr; |
337 | |
338 | chip = ds->priv; |
339 | ps = &chip->port_hwtstamp[port]; |
340 | |
341 | if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT) |
342 | return false; |
343 | |
344 | hdr = mv88e6xxx_should_tstamp(chip, port, skb, type); |
345 | if (!hdr) |
346 | return false; |
347 | |
348 | SKB_PTP_TYPE(skb) = type; |
349 | |
350 | if (is_pdelay_resp(hdr)) |
351 | skb_queue_tail(list: &ps->rx_queue2, newsk: skb); |
352 | else |
353 | skb_queue_tail(list: &ps->rx_queue, newsk: skb); |
354 | |
355 | ptp_schedule_worker(ptp: chip->ptp_clock, delay: 0); |
356 | |
357 | return true; |
358 | } |
359 | |
360 | static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip, |
361 | struct mv88e6xxx_port_hwtstamp *ps) |
362 | { |
363 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
364 | struct skb_shared_hwtstamps shhwtstamps; |
365 | u16 departure_block[4], status; |
366 | struct sk_buff *tmp_skb; |
367 | u32 time_raw; |
368 | int err; |
369 | u64 ns; |
370 | |
371 | if (!ps->tx_skb) |
372 | return 0; |
373 | |
374 | mv88e6xxx_reg_lock(chip); |
375 | err = mv88e6xxx_port_ptp_read(chip, port: ps->port_id, |
376 | addr: ptp_ops->dep_sts_reg, |
377 | data: departure_block, |
378 | ARRAY_SIZE(departure_block)); |
379 | mv88e6xxx_reg_unlock(chip); |
380 | |
381 | if (err) |
382 | goto free_and_clear_skb; |
383 | |
384 | if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) { |
385 | if (time_is_before_jiffies(ps->tx_tstamp_start + |
386 | TX_TSTAMP_TIMEOUT)) { |
387 | dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n" , |
388 | ps->port_id); |
389 | goto free_and_clear_skb; |
390 | } |
391 | /* The timestamp should be available quickly, while getting it |
392 | * is high priority and time bounded to only 10ms. A poll is |
393 | * warranted so restart the work. |
394 | */ |
395 | return 1; |
396 | } |
397 | |
398 | /* We have the timestamp; go ahead and clear valid now */ |
399 | mv88e6xxx_reg_lock(chip); |
400 | mv88e6xxx_port_ptp_write(chip, port: ps->port_id, addr: ptp_ops->dep_sts_reg, data: 0); |
401 | mv88e6xxx_reg_unlock(chip); |
402 | |
403 | status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK; |
404 | if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) { |
405 | dev_warn(chip->dev, "p%d: tx timestamp overrun\n" , ps->port_id); |
406 | goto free_and_clear_skb; |
407 | } |
408 | |
409 | if (departure_block[3] != ps->tx_seq_id) { |
410 | dev_warn(chip->dev, "p%d: unexpected seq. id\n" , ps->port_id); |
411 | goto free_and_clear_skb; |
412 | } |
413 | |
414 | memset(&shhwtstamps, 0, sizeof(shhwtstamps)); |
415 | time_raw = ((u32)departure_block[2] << 16) | departure_block[1]; |
416 | mv88e6xxx_reg_lock(chip); |
417 | ns = timecounter_cyc2time(tc: &chip->tstamp_tc, cycle_tstamp: time_raw); |
418 | mv88e6xxx_reg_unlock(chip); |
419 | shhwtstamps.hwtstamp = ns_to_ktime(ns); |
420 | |
421 | dev_dbg(chip->dev, |
422 | "p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n" , |
423 | ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp), |
424 | departure_block[0], ps->tx_seq_id, departure_block[3]); |
425 | |
426 | /* skb_complete_tx_timestamp() will free up the client to make |
427 | * another timestamp-able transmit. We have to be ready for it |
428 | * -- by clearing the ps->tx_skb "flag" -- beforehand. |
429 | */ |
430 | |
431 | tmp_skb = ps->tx_skb; |
432 | ps->tx_skb = NULL; |
433 | clear_bit_unlock(nr: MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, addr: &ps->state); |
434 | skb_complete_tx_timestamp(skb: tmp_skb, hwtstamps: &shhwtstamps); |
435 | |
436 | return 0; |
437 | |
438 | free_and_clear_skb: |
439 | dev_kfree_skb_any(skb: ps->tx_skb); |
440 | ps->tx_skb = NULL; |
441 | clear_bit_unlock(nr: MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, addr: &ps->state); |
442 | |
443 | return 0; |
444 | } |
445 | |
446 | long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp) |
447 | { |
448 | struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); |
449 | struct dsa_switch *ds = chip->ds; |
450 | struct mv88e6xxx_port_hwtstamp *ps; |
451 | int i, restart = 0; |
452 | |
453 | for (i = 0; i < ds->num_ports; i++) { |
454 | if (!dsa_is_user_port(ds, p: i)) |
455 | continue; |
456 | |
457 | ps = &chip->port_hwtstamp[i]; |
458 | if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) |
459 | restart |= mv88e6xxx_txtstamp_work(chip, ps); |
460 | |
461 | mv88e6xxx_rxtstamp_work(chip, ps); |
462 | } |
463 | |
464 | return restart ? 1 : -1; |
465 | } |
466 | |
467 | void mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port, |
468 | struct sk_buff *skb) |
469 | { |
470 | struct mv88e6xxx_chip *chip = ds->priv; |
471 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; |
472 | struct ptp_header *hdr; |
473 | struct sk_buff *clone; |
474 | unsigned int type; |
475 | |
476 | type = ptp_classify_raw(skb); |
477 | if (type == PTP_CLASS_NONE) |
478 | return; |
479 | |
480 | hdr = mv88e6xxx_should_tstamp(chip, port, skb, type); |
481 | if (!hdr) |
482 | return; |
483 | |
484 | clone = skb_clone_sk(skb); |
485 | if (!clone) |
486 | return; |
487 | |
488 | if (test_and_set_bit_lock(nr: MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, |
489 | addr: &ps->state)) { |
490 | kfree_skb(skb: clone); |
491 | return; |
492 | } |
493 | |
494 | ps->tx_skb = clone; |
495 | ps->tx_tstamp_start = jiffies; |
496 | ps->tx_seq_id = be16_to_cpu(hdr->sequence_id); |
497 | |
498 | ptp_schedule_worker(ptp: chip->ptp_clock, delay: 0); |
499 | } |
500 | |
501 | int mv88e6165_global_disable(struct mv88e6xxx_chip *chip) |
502 | { |
503 | u16 val; |
504 | int err; |
505 | |
506 | err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, data: &val); |
507 | if (err) |
508 | return err; |
509 | val |= MV88E6165_PTP_CFG_DISABLE_PTP; |
510 | |
511 | return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, data: val); |
512 | } |
513 | |
514 | int mv88e6165_global_enable(struct mv88e6xxx_chip *chip) |
515 | { |
516 | u16 val; |
517 | int err; |
518 | |
519 | err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, data: &val); |
520 | if (err) |
521 | return err; |
522 | |
523 | val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP | MV88E6165_PTP_CFG_TSPEC_MASK); |
524 | |
525 | return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, data: val); |
526 | } |
527 | |
528 | int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port) |
529 | { |
530 | return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, |
531 | MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP); |
532 | } |
533 | |
534 | int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port) |
535 | { |
536 | return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, |
537 | MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH); |
538 | } |
539 | |
540 | static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port) |
541 | { |
542 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
543 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; |
544 | |
545 | ps->port_id = port; |
546 | |
547 | skb_queue_head_init(list: &ps->rx_queue); |
548 | skb_queue_head_init(list: &ps->rx_queue2); |
549 | |
550 | if (ptp_ops->port_disable) |
551 | return ptp_ops->port_disable(chip, port); |
552 | |
553 | return 0; |
554 | } |
555 | |
556 | int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip) |
557 | { |
558 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
559 | int err; |
560 | int i; |
561 | |
562 | /* Disable timestamping on all ports. */ |
563 | for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) { |
564 | err = mv88e6xxx_hwtstamp_port_setup(chip, port: i); |
565 | if (err) |
566 | return err; |
567 | } |
568 | |
569 | /* Disable PTP globally */ |
570 | if (ptp_ops->global_disable) { |
571 | err = ptp_ops->global_disable(chip); |
572 | if (err) |
573 | return err; |
574 | } |
575 | |
576 | /* Set the ethertype of L2 PTP messages */ |
577 | err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_GC_ETYPE, ETH_P_1588); |
578 | if (err) |
579 | return err; |
580 | |
581 | /* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to |
582 | * timestamp. This affects all ports that have timestamping enabled, |
583 | * but the timestamp config is per-port; thus we configure all events |
584 | * here and only support the HWTSTAMP_FILTER_*_EVENT filter types. |
585 | */ |
586 | err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE, |
587 | MV88E6XXX_PTP_MSGTYPE_ALL_EVENT); |
588 | if (err) |
589 | return err; |
590 | |
591 | /* Use ARRIVAL1 for peer delay response messages. */ |
592 | err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR, |
593 | MV88E6XXX_PTP_MSGTYPE_PDLAY_RES); |
594 | if (err) |
595 | return err; |
596 | |
597 | /* 88E6341 devices default to timestamping at the PHY, but this has |
598 | * a hardware issue that results in unreliable timestamps. Force |
599 | * these devices to timestamp at the MAC. |
600 | */ |
601 | if (chip->info->family == MV88E6XXX_FAMILY_6341) { |
602 | u16 val = MV88E6341_PTP_CFG_UPDATE | |
603 | MV88E6341_PTP_CFG_MODE_IDX | |
604 | MV88E6341_PTP_CFG_MODE_TS_AT_MAC; |
605 | err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, data: val); |
606 | if (err) |
607 | return err; |
608 | } |
609 | |
610 | return 0; |
611 | } |
612 | |
613 | void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip) |
614 | { |
615 | } |
616 | |