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