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