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