]>
Commit | Line | Data |
---|---|---|
2874c5fd | 1 | // SPDX-License-Identifier: GPL-2.0-or-later |
c6fe0ad2 BS |
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> | |
c6fe0ad2 BS |
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 | ||
e2294a8b AL |
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 | ||
c6fe0ad2 BS |
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 | */ | |
df31b74c | 64 | #define TX_TSTAMP_TIMEOUT msecs_to_jiffies(40) |
c6fe0ad2 BS |
65 | |
66 | int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port, | |
67 | struct ethtool_ts_info *info) | |
68 | { | |
48cb5e03 AL |
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; | |
c6fe0ad2 BS |
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(chip->ptp_clock); | |
83 | info->tx_types = | |
84 | (1 << HWTSTAMP_TX_OFF) | | |
85 | (1 << HWTSTAMP_TX_ON); | |
48cb5e03 | 86 | info->rx_filters = ptp_ops->rx_filters; |
c6fe0ad2 BS |
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 | { | |
ffc705de | 94 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
c6fe0ad2 BS |
95 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; |
96 | bool tstamp_enable = false; | |
c6fe0ad2 BS |
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(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state); | |
102 | ||
103 | /* reserved for future extensions */ | |
104 | if (config->flags) | |
105 | return -EINVAL; | |
106 | ||
107 | switch (config->tx_type) { | |
108 | case HWTSTAMP_TX_OFF: | |
109 | tstamp_enable = false; | |
110 | break; | |
111 | case HWTSTAMP_TX_ON: | |
112 | tstamp_enable = true; | |
113 | break; | |
114 | default: | |
115 | return -ERANGE; | |
116 | } | |
117 | ||
118 | /* The switch supports timestamping both L2 and L4; one cannot be | |
119 | * disabled independently of the other. | |
120 | */ | |
48cb5e03 AL |
121 | |
122 | if (!(BIT(config->rx_filter) & ptp_ops->rx_filters)) { | |
123 | config->rx_filter = HWTSTAMP_FILTER_NONE; | |
124 | dev_dbg(chip->dev, "Unsupported rx_filter %d\n", | |
125 | config->rx_filter); | |
126 | return -ERANGE; | |
127 | } | |
128 | ||
c6fe0ad2 BS |
129 | switch (config->rx_filter) { |
130 | case HWTSTAMP_FILTER_NONE: | |
131 | tstamp_enable = false; | |
132 | break; | |
133 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: | |
134 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: | |
135 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: | |
136 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: | |
137 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: | |
138 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: | |
139 | case HWTSTAMP_FILTER_PTP_V2_EVENT: | |
140 | case HWTSTAMP_FILTER_PTP_V2_SYNC: | |
141 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: | |
142 | config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; | |
143 | break; | |
144 | case HWTSTAMP_FILTER_ALL: | |
145 | default: | |
146 | config->rx_filter = HWTSTAMP_FILTER_NONE; | |
147 | return -ERANGE; | |
148 | } | |
149 | ||
ffc705de | 150 | mutex_lock(&chip->reg_lock); |
c6fe0ad2 | 151 | if (tstamp_enable) { |
e2294a8b AL |
152 | chip->enable_count += 1; |
153 | if (chip->enable_count == 1 && ptp_ops->global_enable) | |
154 | ptp_ops->global_enable(chip); | |
ffc705de AL |
155 | if (ptp_ops->port_enable) |
156 | ptp_ops->port_enable(chip, port); | |
c6fe0ad2 | 157 | } else { |
ffc705de AL |
158 | if (ptp_ops->port_disable) |
159 | ptp_ops->port_disable(chip, port); | |
e2294a8b AL |
160 | chip->enable_count -= 1; |
161 | if (chip->enable_count == 0 && ptp_ops->global_disable) | |
162 | ptp_ops->global_disable(chip); | |
c6fe0ad2 | 163 | } |
c6fe0ad2 BS |
164 | mutex_unlock(&chip->reg_lock); |
165 | ||
c6fe0ad2 BS |
166 | /* Once hardware has been configured, enable timestamp checks |
167 | * in the RX/TX paths. | |
168 | */ | |
169 | if (tstamp_enable) | |
170 | set_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state); | |
171 | ||
172 | return 0; | |
173 | } | |
174 | ||
175 | int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port, | |
176 | struct ifreq *ifr) | |
177 | { | |
178 | struct mv88e6xxx_chip *chip = ds->priv; | |
179 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; | |
180 | struct hwtstamp_config config; | |
181 | int err; | |
182 | ||
183 | if (!chip->info->ptp_support) | |
184 | return -EOPNOTSUPP; | |
185 | ||
c6fe0ad2 BS |
186 | if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) |
187 | return -EFAULT; | |
188 | ||
189 | err = mv88e6xxx_set_hwtstamp_config(chip, port, &config); | |
190 | if (err) | |
191 | return err; | |
192 | ||
193 | /* Save the chosen configuration to be returned later. */ | |
194 | memcpy(&ps->tstamp_config, &config, sizeof(config)); | |
195 | ||
196 | return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? | |
197 | -EFAULT : 0; | |
198 | } | |
199 | ||
200 | int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port, | |
201 | struct ifreq *ifr) | |
202 | { | |
203 | struct mv88e6xxx_chip *chip = ds->priv; | |
204 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; | |
205 | struct hwtstamp_config *config = &ps->tstamp_config; | |
206 | ||
207 | if (!chip->info->ptp_support) | |
208 | return -EOPNOTSUPP; | |
209 | ||
c6fe0ad2 BS |
210 | return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ? |
211 | -EFAULT : 0; | |
212 | } | |
213 | ||
214 | /* Get the start of the PTP header in this skb */ | |
215 | static u8 *parse_ptp_header(struct sk_buff *skb, unsigned int type) | |
216 | { | |
217 | u8 *data = skb_mac_header(skb); | |
218 | unsigned int offset = 0; | |
219 | ||
220 | if (type & PTP_CLASS_VLAN) | |
221 | offset += VLAN_HLEN; | |
222 | ||
223 | switch (type & PTP_CLASS_PMASK) { | |
224 | case PTP_CLASS_IPV4: | |
225 | offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; | |
226 | break; | |
227 | case PTP_CLASS_IPV6: | |
228 | offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; | |
229 | break; | |
230 | case PTP_CLASS_L2: | |
231 | offset += ETH_HLEN; | |
232 | break; | |
233 | default: | |
234 | return NULL; | |
235 | } | |
236 | ||
237 | /* Ensure that the entire header is present in this packet. */ | |
238 | if (skb->len + ETH_HLEN < offset + 34) | |
239 | return NULL; | |
240 | ||
241 | return data + offset; | |
242 | } | |
243 | ||
244 | /* Returns a pointer to the PTP header if the caller should time stamp, | |
245 | * or NULL if the caller should not. | |
246 | */ | |
247 | static u8 *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, int port, | |
248 | struct sk_buff *skb, unsigned int type) | |
249 | { | |
250 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; | |
251 | u8 *hdr; | |
252 | ||
253 | if (!chip->info->ptp_support) | |
254 | return NULL; | |
255 | ||
c6fe0ad2 BS |
256 | hdr = parse_ptp_header(skb, type); |
257 | if (!hdr) | |
258 | return NULL; | |
259 | ||
260 | if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state)) | |
261 | return NULL; | |
262 | ||
263 | return hdr; | |
264 | } | |
265 | ||
266 | static int mv88e6xxx_ts_valid(u16 status) | |
267 | { | |
268 | if (!(status & MV88E6XXX_PTP_TS_VALID)) | |
269 | return 0; | |
270 | if (status & MV88E6XXX_PTP_TS_STATUS_MASK) | |
271 | return 0; | |
272 | return 1; | |
273 | } | |
274 | ||
275 | static int seq_match(struct sk_buff *skb, u16 ts_seqid) | |
276 | { | |
277 | unsigned int type = SKB_PTP_TYPE(skb); | |
278 | u8 *hdr = parse_ptp_header(skb, type); | |
279 | __be16 *seqid; | |
280 | ||
281 | seqid = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID); | |
282 | ||
283 | return ts_seqid == ntohs(*seqid); | |
284 | } | |
285 | ||
286 | static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip, | |
287 | struct mv88e6xxx_port_hwtstamp *ps, | |
288 | struct sk_buff *skb, u16 reg, | |
289 | struct sk_buff_head *rxq) | |
290 | { | |
b2d12101 | 291 | u16 buf[4] = { 0 }, status, seq_id; |
c6fe0ad2 | 292 | struct skb_shared_hwtstamps *shwt; |
22904823 RC |
293 | struct sk_buff_head received; |
294 | u64 ns, timelo, timehi; | |
295 | unsigned long flags; | |
c6fe0ad2 BS |
296 | int err; |
297 | ||
22904823 RC |
298 | /* The latched timestamp belongs to one of the received frames. */ |
299 | __skb_queue_head_init(&received); | |
300 | spin_lock_irqsave(&rxq->lock, flags); | |
301 | skb_queue_splice_tail_init(rxq, &received); | |
302 | spin_unlock_irqrestore(&rxq->lock, flags); | |
303 | ||
c6fe0ad2 BS |
304 | mutex_lock(&chip->reg_lock); |
305 | err = mv88e6xxx_port_ptp_read(chip, ps->port_id, | |
306 | reg, buf, ARRAY_SIZE(buf)); | |
307 | mutex_unlock(&chip->reg_lock); | |
308 | if (err) | |
309 | pr_err("failed to get the receive time stamp\n"); | |
310 | ||
311 | status = buf[0]; | |
312 | timelo = buf[1]; | |
313 | timehi = buf[2]; | |
314 | seq_id = buf[3]; | |
315 | ||
316 | if (status & MV88E6XXX_PTP_TS_VALID) { | |
317 | mutex_lock(&chip->reg_lock); | |
318 | err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0); | |
319 | mutex_unlock(&chip->reg_lock); | |
320 | if (err) | |
321 | pr_err("failed to clear the receive status\n"); | |
322 | } | |
323 | /* Since the device can only handle one time stamp at a time, | |
324 | * we purge any extra frames from the queue. | |
325 | */ | |
22904823 | 326 | for ( ; skb; skb = __skb_dequeue(&received)) { |
c6fe0ad2 | 327 | if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) { |
b2d12101 | 328 | ns = timehi << 16 | timelo; |
c6fe0ad2 BS |
329 | |
330 | mutex_lock(&chip->reg_lock); | |
331 | ns = timecounter_cyc2time(&chip->tstamp_tc, ns); | |
332 | mutex_unlock(&chip->reg_lock); | |
333 | shwt = skb_hwtstamps(skb); | |
334 | memset(shwt, 0, sizeof(*shwt)); | |
335 | shwt->hwtstamp = ns_to_ktime(ns); | |
336 | status &= ~MV88E6XXX_PTP_TS_VALID; | |
337 | } | |
338 | netif_rx_ni(skb); | |
339 | } | |
340 | } | |
341 | ||
342 | static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip, | |
343 | struct mv88e6xxx_port_hwtstamp *ps) | |
344 | { | |
ffc705de | 345 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
c6fe0ad2 BS |
346 | struct sk_buff *skb; |
347 | ||
348 | skb = skb_dequeue(&ps->rx_queue); | |
349 | ||
350 | if (skb) | |
ffc705de | 351 | mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr0_sts_reg, |
c6fe0ad2 BS |
352 | &ps->rx_queue); |
353 | ||
354 | skb = skb_dequeue(&ps->rx_queue2); | |
355 | if (skb) | |
ffc705de | 356 | mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr1_sts_reg, |
c6fe0ad2 BS |
357 | &ps->rx_queue2); |
358 | } | |
359 | ||
360 | static int is_pdelay_resp(u8 *msgtype) | |
361 | { | |
362 | return (*msgtype & 0xf) == 3; | |
363 | } | |
364 | ||
365 | bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port, | |
366 | struct sk_buff *skb, unsigned int type) | |
367 | { | |
368 | struct mv88e6xxx_port_hwtstamp *ps; | |
369 | struct mv88e6xxx_chip *chip; | |
370 | u8 *hdr; | |
371 | ||
372 | chip = ds->priv; | |
373 | ps = &chip->port_hwtstamp[port]; | |
374 | ||
375 | if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT) | |
376 | return false; | |
377 | ||
378 | hdr = mv88e6xxx_should_tstamp(chip, port, skb, type); | |
379 | if (!hdr) | |
380 | return false; | |
381 | ||
382 | SKB_PTP_TYPE(skb) = type; | |
383 | ||
384 | if (is_pdelay_resp(hdr)) | |
385 | skb_queue_tail(&ps->rx_queue2, skb); | |
386 | else | |
387 | skb_queue_tail(&ps->rx_queue, skb); | |
388 | ||
389 | ptp_schedule_worker(chip->ptp_clock, 0); | |
390 | ||
391 | return true; | |
392 | } | |
393 | ||
394 | static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip, | |
395 | struct mv88e6xxx_port_hwtstamp *ps) | |
396 | { | |
ffc705de | 397 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
c6fe0ad2 BS |
398 | struct skb_shared_hwtstamps shhwtstamps; |
399 | u16 departure_block[4], status; | |
400 | struct sk_buff *tmp_skb; | |
401 | u32 time_raw; | |
402 | int err; | |
403 | u64 ns; | |
404 | ||
405 | if (!ps->tx_skb) | |
406 | return 0; | |
407 | ||
408 | mutex_lock(&chip->reg_lock); | |
409 | err = mv88e6xxx_port_ptp_read(chip, ps->port_id, | |
ffc705de | 410 | ptp_ops->dep_sts_reg, |
c6fe0ad2 BS |
411 | departure_block, |
412 | ARRAY_SIZE(departure_block)); | |
413 | mutex_unlock(&chip->reg_lock); | |
414 | ||
415 | if (err) | |
416 | goto free_and_clear_skb; | |
417 | ||
418 | if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) { | |
419 | if (time_is_before_jiffies(ps->tx_tstamp_start + | |
420 | TX_TSTAMP_TIMEOUT)) { | |
421 | dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n", | |
422 | ps->port_id); | |
423 | goto free_and_clear_skb; | |
424 | } | |
425 | /* The timestamp should be available quickly, while getting it | |
426 | * is high priority and time bounded to only 10ms. A poll is | |
427 | * warranted so restart the work. | |
428 | */ | |
429 | return 1; | |
430 | } | |
431 | ||
432 | /* We have the timestamp; go ahead and clear valid now */ | |
433 | mutex_lock(&chip->reg_lock); | |
ffc705de | 434 | mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0); |
c6fe0ad2 BS |
435 | mutex_unlock(&chip->reg_lock); |
436 | ||
437 | status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK; | |
438 | if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) { | |
439 | dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id); | |
440 | goto free_and_clear_skb; | |
441 | } | |
442 | ||
443 | if (departure_block[3] != ps->tx_seq_id) { | |
444 | dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id); | |
445 | goto free_and_clear_skb; | |
446 | } | |
447 | ||
448 | memset(&shhwtstamps, 0, sizeof(shhwtstamps)); | |
449 | time_raw = ((u32)departure_block[2] << 16) | departure_block[1]; | |
450 | mutex_lock(&chip->reg_lock); | |
451 | ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw); | |
452 | mutex_unlock(&chip->reg_lock); | |
453 | shhwtstamps.hwtstamp = ns_to_ktime(ns); | |
454 | ||
455 | dev_dbg(chip->dev, | |
456 | "p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n", | |
457 | ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp), | |
458 | departure_block[0], ps->tx_seq_id, departure_block[3]); | |
459 | ||
460 | /* skb_complete_tx_timestamp() will free up the client to make | |
461 | * another timestamp-able transmit. We have to be ready for it | |
462 | * -- by clearing the ps->tx_skb "flag" -- beforehand. | |
463 | */ | |
464 | ||
465 | tmp_skb = ps->tx_skb; | |
466 | ps->tx_skb = NULL; | |
467 | clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state); | |
468 | skb_complete_tx_timestamp(tmp_skb, &shhwtstamps); | |
469 | ||
470 | return 0; | |
471 | ||
472 | free_and_clear_skb: | |
473 | dev_kfree_skb_any(ps->tx_skb); | |
474 | ps->tx_skb = NULL; | |
475 | clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state); | |
476 | ||
477 | return 0; | |
478 | } | |
479 | ||
480 | long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp) | |
481 | { | |
482 | struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); | |
483 | struct dsa_switch *ds = chip->ds; | |
484 | struct mv88e6xxx_port_hwtstamp *ps; | |
485 | int i, restart = 0; | |
486 | ||
487 | for (i = 0; i < ds->num_ports; i++) { | |
488 | if (!dsa_is_user_port(ds, i)) | |
489 | continue; | |
490 | ||
491 | ps = &chip->port_hwtstamp[i]; | |
492 | if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) | |
493 | restart |= mv88e6xxx_txtstamp_work(chip, ps); | |
494 | ||
495 | mv88e6xxx_rxtstamp_work(chip, ps); | |
496 | } | |
497 | ||
498 | return restart ? 1 : -1; | |
499 | } | |
500 | ||
501 | bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port, | |
502 | struct sk_buff *clone, unsigned int type) | |
503 | { | |
504 | struct mv88e6xxx_chip *chip = ds->priv; | |
505 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; | |
506 | __be16 *seq_ptr; | |
507 | u8 *hdr; | |
508 | ||
509 | if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP)) | |
510 | return false; | |
511 | ||
512 | hdr = mv88e6xxx_should_tstamp(chip, port, clone, type); | |
513 | if (!hdr) | |
514 | return false; | |
515 | ||
516 | seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID); | |
517 | ||
518 | if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, | |
519 | &ps->state)) | |
520 | return false; | |
521 | ||
522 | ps->tx_skb = clone; | |
523 | ps->tx_tstamp_start = jiffies; | |
524 | ps->tx_seq_id = be16_to_cpup(seq_ptr); | |
525 | ||
526 | ptp_schedule_worker(chip->ptp_clock, 0); | |
527 | return true; | |
528 | } | |
529 | ||
e2294a8b AL |
530 | int mv88e6165_global_disable(struct mv88e6xxx_chip *chip) |
531 | { | |
532 | u16 val; | |
533 | int err; | |
534 | ||
535 | err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val); | |
536 | if (err) | |
537 | return err; | |
538 | val |= MV88E6165_PTP_CFG_DISABLE_PTP; | |
539 | ||
540 | return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val); | |
541 | } | |
542 | ||
543 | int mv88e6165_global_enable(struct mv88e6xxx_chip *chip) | |
544 | { | |
545 | u16 val; | |
546 | int err; | |
547 | ||
548 | err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val); | |
549 | if (err) | |
550 | return err; | |
551 | ||
552 | val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP | MV88E6165_PTP_CFG_TSPEC_MASK); | |
553 | ||
554 | return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val); | |
555 | } | |
556 | ||
ffc705de AL |
557 | int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port) |
558 | { | |
559 | return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, | |
560 | MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP); | |
561 | } | |
562 | ||
563 | int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port) | |
564 | { | |
565 | return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, | |
566 | MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH); | |
567 | } | |
568 | ||
c6fe0ad2 BS |
569 | static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port) |
570 | { | |
ffc705de | 571 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
c6fe0ad2 BS |
572 | struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; |
573 | ||
574 | ps->port_id = port; | |
575 | ||
576 | skb_queue_head_init(&ps->rx_queue); | |
577 | skb_queue_head_init(&ps->rx_queue2); | |
578 | ||
ffc705de AL |
579 | if (ptp_ops->port_disable) |
580 | return ptp_ops->port_disable(chip, port); | |
581 | ||
582 | return 0; | |
c6fe0ad2 BS |
583 | } |
584 | ||
585 | int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip) | |
586 | { | |
e2294a8b | 587 | const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; |
c6fe0ad2 BS |
588 | int err; |
589 | int i; | |
590 | ||
591 | /* Disable timestamping on all ports. */ | |
592 | for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) { | |
593 | err = mv88e6xxx_hwtstamp_port_setup(chip, i); | |
594 | if (err) | |
595 | return err; | |
596 | } | |
597 | ||
e2294a8b AL |
598 | /* Disable PTP globally */ |
599 | if (ptp_ops->global_disable) { | |
600 | err = ptp_ops->global_disable(chip); | |
601 | if (err) | |
602 | return err; | |
603 | } | |
604 | ||
2dbed245 AL |
605 | /* Set the ethertype of L2 PTP messages */ |
606 | err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_GC_ETYPE, ETH_P_1588); | |
607 | if (err) | |
608 | return err; | |
609 | ||
c6fe0ad2 BS |
610 | /* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to |
611 | * timestamp. This affects all ports that have timestamping enabled, | |
612 | * but the timestamp config is per-port; thus we configure all events | |
613 | * here and only support the HWTSTAMP_FILTER_*_EVENT filter types. | |
614 | */ | |
615 | err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE, | |
616 | MV88E6XXX_PTP_MSGTYPE_ALL_EVENT); | |
617 | if (err) | |
618 | return err; | |
619 | ||
620 | /* Use ARRIVAL1 for peer delay response messages. */ | |
621 | err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR, | |
622 | MV88E6XXX_PTP_MSGTYPE_PDLAY_RES); | |
623 | if (err) | |
624 | return err; | |
625 | ||
a2e47134 BS |
626 | /* 88E6341 devices default to timestamping at the PHY, but this has |
627 | * a hardware issue that results in unreliable timestamps. Force | |
628 | * these devices to timestamp at the MAC. | |
629 | */ | |
630 | if (chip->info->family == MV88E6XXX_FAMILY_6341) { | |
631 | u16 val = MV88E6341_PTP_CFG_UPDATE | | |
632 | MV88E6341_PTP_CFG_MODE_IDX | | |
633 | MV88E6341_PTP_CFG_MODE_TS_AT_MAC; | |
634 | err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val); | |
635 | if (err) | |
636 | return err; | |
637 | } | |
638 | ||
c6fe0ad2 BS |
639 | return 0; |
640 | } | |
641 | ||
642 | void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip) | |
643 | { | |
644 | } |