1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | |
3 | #include <linux/ptp_classify.h> |
4 | |
5 | #include "lan966x_main.h" |
6 | #include "vcap_api.h" |
7 | #include "vcap_api_client.h" |
8 | |
9 | #define LAN966X_MAX_PTP_ID 512 |
10 | |
11 | /* Represents 1ppm adjustment in 2^59 format with 6.037735849ns as reference |
12 | * The value is calculated as following: (1/1000000)/((2^-59)/6.037735849) |
13 | */ |
14 | #define LAN966X_1PPM_FORMAT 3480517749723LL |
15 | |
16 | /* Represents 1ppb adjustment in 2^29 format with 6.037735849ns as reference |
17 | * The value is calculated as following: (1/1000000000)/((2^59)/6.037735849) |
18 | */ |
19 | #define LAN966X_1PPB_FORMAT 3480517749LL |
20 | |
21 | #define TOD_ACC_PIN 0x7 |
22 | |
23 | /* This represents the base rule ID for the PTP rules that are added in the |
24 | * VCAP to trap frames to CPU. This number needs to be bigger than the maximum |
25 | * number of entries that can exist in the VCAP. |
26 | */ |
27 | #define LAN966X_VCAP_PTP_RULE_ID 1000000 |
28 | #define LAN966X_VCAP_L2_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 0) |
29 | #define LAN966X_VCAP_IPV4_EV_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 1) |
30 | #define LAN966X_VCAP_IPV4_GEN_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 2) |
31 | #define LAN966X_VCAP_IPV6_EV_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 3) |
32 | #define LAN966X_VCAP_IPV6_GEN_PTP_TRAP (LAN966X_VCAP_PTP_RULE_ID + 4) |
33 | |
34 | enum { |
35 | PTP_PIN_ACTION_IDLE = 0, |
36 | PTP_PIN_ACTION_LOAD, |
37 | PTP_PIN_ACTION_SAVE, |
38 | PTP_PIN_ACTION_CLOCK, |
39 | PTP_PIN_ACTION_DELTA, |
40 | PTP_PIN_ACTION_TOD |
41 | }; |
42 | |
43 | static u64 lan966x_ptp_get_nominal_value(void) |
44 | { |
45 | /* This is the default value that for each system clock, the time of day |
46 | * is increased. It has the format 5.59 nanosecond. |
47 | */ |
48 | return 0x304d4873ecade305; |
49 | } |
50 | |
51 | static int lan966x_ptp_add_trap(struct lan966x_port *port, |
52 | int (*add_ptp_key)(struct vcap_rule *vrule, |
53 | struct lan966x_port*), |
54 | u32 rule_id, |
55 | u16 proto) |
56 | { |
57 | struct lan966x *lan966x = port->lan966x; |
58 | struct vcap_rule *vrule; |
59 | int err; |
60 | |
61 | vrule = vcap_get_rule(vctrl: lan966x->vcap_ctrl, id: rule_id); |
62 | if (!IS_ERR(ptr: vrule)) { |
63 | u32 value, mask; |
64 | |
65 | /* Just modify the ingress port mask and exit */ |
66 | vcap_rule_get_key_u32(rule: vrule, key: VCAP_KF_IF_IGR_PORT_MASK, |
67 | value: &value, mask: &mask); |
68 | mask &= ~BIT(port->chip_port); |
69 | vcap_rule_mod_key_u32(rule: vrule, key: VCAP_KF_IF_IGR_PORT_MASK, |
70 | value, mask); |
71 | |
72 | err = vcap_mod_rule(rule: vrule); |
73 | goto free_rule; |
74 | } |
75 | |
76 | vrule = vcap_alloc_rule(vctrl: lan966x->vcap_ctrl, ndev: port->dev, |
77 | LAN966X_VCAP_CID_IS2_L0, |
78 | user: VCAP_USER_PTP, priority: 0, id: rule_id); |
79 | if (IS_ERR(ptr: vrule)) |
80 | return PTR_ERR(ptr: vrule); |
81 | |
82 | err = add_ptp_key(vrule, port); |
83 | if (err) |
84 | goto free_rule; |
85 | |
86 | err = vcap_rule_add_action_bit(rule: vrule, action: VCAP_AF_CPU_COPY_ENA, val: VCAP_BIT_1); |
87 | err |= vcap_rule_add_action_u32(rule: vrule, action: VCAP_AF_MASK_MODE, value: LAN966X_PMM_REPLACE); |
88 | err |= vcap_val_rule(rule: vrule, l3_proto: proto); |
89 | if (err) |
90 | goto free_rule; |
91 | |
92 | err = vcap_add_rule(rule: vrule); |
93 | |
94 | free_rule: |
95 | /* Free the local copy of the rule */ |
96 | vcap_free_rule(rule: vrule); |
97 | return err; |
98 | } |
99 | |
100 | static int lan966x_ptp_del_trap(struct lan966x_port *port, |
101 | u32 rule_id) |
102 | { |
103 | struct lan966x *lan966x = port->lan966x; |
104 | struct vcap_rule *vrule; |
105 | u32 value, mask; |
106 | int err; |
107 | |
108 | vrule = vcap_get_rule(vctrl: lan966x->vcap_ctrl, id: rule_id); |
109 | if (IS_ERR(ptr: vrule)) |
110 | return -EEXIST; |
111 | |
112 | vcap_rule_get_key_u32(rule: vrule, key: VCAP_KF_IF_IGR_PORT_MASK, value: &value, mask: &mask); |
113 | mask |= BIT(port->chip_port); |
114 | |
115 | /* No other port requires this trap, so it is safe to remove it */ |
116 | if (mask == GENMASK(lan966x->num_phys_ports, 0)) { |
117 | err = vcap_del_rule(vctrl: lan966x->vcap_ctrl, ndev: port->dev, id: rule_id); |
118 | goto free_rule; |
119 | } |
120 | |
121 | vcap_rule_mod_key_u32(rule: vrule, key: VCAP_KF_IF_IGR_PORT_MASK, value, mask); |
122 | err = vcap_mod_rule(rule: vrule); |
123 | |
124 | free_rule: |
125 | vcap_free_rule(rule: vrule); |
126 | return err; |
127 | } |
128 | |
129 | static int lan966x_ptp_add_l2_key(struct vcap_rule *vrule, |
130 | struct lan966x_port *port) |
131 | { |
132 | return vcap_rule_add_key_u32(rule: vrule, key: VCAP_KF_ETYPE, ETH_P_1588, mask: ~0); |
133 | } |
134 | |
135 | static int lan966x_ptp_add_ip_event_key(struct vcap_rule *vrule, |
136 | struct lan966x_port *port) |
137 | { |
138 | return vcap_rule_add_key_u32(rule: vrule, key: VCAP_KF_L4_DPORT, PTP_EV_PORT, mask: ~0) || |
139 | vcap_rule_add_key_bit(rule: vrule, key: VCAP_KF_TCP_IS, val: VCAP_BIT_0); |
140 | } |
141 | |
142 | static int lan966x_ptp_add_ip_general_key(struct vcap_rule *vrule, |
143 | struct lan966x_port *port) |
144 | { |
145 | return vcap_rule_add_key_u32(rule: vrule, key: VCAP_KF_L4_DPORT, PTP_GEN_PORT, mask: ~0) || |
146 | vcap_rule_add_key_bit(rule: vrule, key: VCAP_KF_TCP_IS, val: VCAP_BIT_0); |
147 | } |
148 | |
149 | static int lan966x_ptp_add_l2_rule(struct lan966x_port *port) |
150 | { |
151 | return lan966x_ptp_add_trap(port, add_ptp_key: lan966x_ptp_add_l2_key, |
152 | LAN966X_VCAP_L2_PTP_TRAP, ETH_P_ALL); |
153 | } |
154 | |
155 | static int lan966x_ptp_add_ipv4_rules(struct lan966x_port *port) |
156 | { |
157 | int err; |
158 | |
159 | err = lan966x_ptp_add_trap(port, add_ptp_key: lan966x_ptp_add_ip_event_key, |
160 | LAN966X_VCAP_IPV4_EV_PTP_TRAP, ETH_P_IP); |
161 | if (err) |
162 | return err; |
163 | |
164 | err = lan966x_ptp_add_trap(port, add_ptp_key: lan966x_ptp_add_ip_general_key, |
165 | LAN966X_VCAP_IPV4_GEN_PTP_TRAP, ETH_P_IP); |
166 | if (err) |
167 | lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_EV_PTP_TRAP); |
168 | |
169 | return err; |
170 | } |
171 | |
172 | static int lan966x_ptp_add_ipv6_rules(struct lan966x_port *port) |
173 | { |
174 | int err; |
175 | |
176 | err = lan966x_ptp_add_trap(port, add_ptp_key: lan966x_ptp_add_ip_event_key, |
177 | LAN966X_VCAP_IPV6_EV_PTP_TRAP, ETH_P_IPV6); |
178 | if (err) |
179 | return err; |
180 | |
181 | err = lan966x_ptp_add_trap(port, add_ptp_key: lan966x_ptp_add_ip_general_key, |
182 | LAN966X_VCAP_IPV6_GEN_PTP_TRAP, ETH_P_IPV6); |
183 | if (err) |
184 | lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_EV_PTP_TRAP); |
185 | |
186 | return err; |
187 | } |
188 | |
189 | static int lan966x_ptp_del_l2_rule(struct lan966x_port *port) |
190 | { |
191 | return lan966x_ptp_del_trap(port, LAN966X_VCAP_L2_PTP_TRAP); |
192 | } |
193 | |
194 | static int lan966x_ptp_del_ipv4_rules(struct lan966x_port *port) |
195 | { |
196 | int err; |
197 | |
198 | err = lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_EV_PTP_TRAP); |
199 | err |= lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV4_GEN_PTP_TRAP); |
200 | |
201 | return err; |
202 | } |
203 | |
204 | static int lan966x_ptp_del_ipv6_rules(struct lan966x_port *port) |
205 | { |
206 | int err; |
207 | |
208 | err = lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_EV_PTP_TRAP); |
209 | err |= lan966x_ptp_del_trap(port, LAN966X_VCAP_IPV6_GEN_PTP_TRAP); |
210 | |
211 | return err; |
212 | } |
213 | |
214 | static int lan966x_ptp_add_traps(struct lan966x_port *port) |
215 | { |
216 | int err; |
217 | |
218 | err = lan966x_ptp_add_l2_rule(port); |
219 | if (err) |
220 | goto err_l2; |
221 | |
222 | err = lan966x_ptp_add_ipv4_rules(port); |
223 | if (err) |
224 | goto err_ipv4; |
225 | |
226 | err = lan966x_ptp_add_ipv6_rules(port); |
227 | if (err) |
228 | goto err_ipv6; |
229 | |
230 | return err; |
231 | |
232 | err_ipv6: |
233 | lan966x_ptp_del_ipv4_rules(port); |
234 | err_ipv4: |
235 | lan966x_ptp_del_l2_rule(port); |
236 | err_l2: |
237 | return err; |
238 | } |
239 | |
240 | int lan966x_ptp_del_traps(struct lan966x_port *port) |
241 | { |
242 | int err; |
243 | |
244 | err = lan966x_ptp_del_l2_rule(port); |
245 | err |= lan966x_ptp_del_ipv4_rules(port); |
246 | err |= lan966x_ptp_del_ipv6_rules(port); |
247 | |
248 | return err; |
249 | } |
250 | |
251 | int lan966x_ptp_setup_traps(struct lan966x_port *port, |
252 | struct kernel_hwtstamp_config *cfg) |
253 | { |
254 | if (cfg->rx_filter == HWTSTAMP_FILTER_NONE) |
255 | return lan966x_ptp_del_traps(port); |
256 | else |
257 | return lan966x_ptp_add_traps(port); |
258 | } |
259 | |
260 | int lan966x_ptp_hwtstamp_set(struct lan966x_port *port, |
261 | struct kernel_hwtstamp_config *cfg, |
262 | struct netlink_ext_ack *extack) |
263 | { |
264 | struct lan966x *lan966x = port->lan966x; |
265 | struct lan966x_phc *phc; |
266 | |
267 | switch (cfg->tx_type) { |
268 | case HWTSTAMP_TX_ON: |
269 | port->ptp_tx_cmd = IFH_REW_OP_TWO_STEP_PTP; |
270 | break; |
271 | case HWTSTAMP_TX_ONESTEP_SYNC: |
272 | port->ptp_tx_cmd = IFH_REW_OP_ONE_STEP_PTP; |
273 | break; |
274 | case HWTSTAMP_TX_OFF: |
275 | port->ptp_tx_cmd = IFH_REW_OP_NOOP; |
276 | break; |
277 | default: |
278 | return -ERANGE; |
279 | } |
280 | |
281 | switch (cfg->rx_filter) { |
282 | case HWTSTAMP_FILTER_NONE: |
283 | port->ptp_rx_cmd = false; |
284 | break; |
285 | case HWTSTAMP_FILTER_ALL: |
286 | case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: |
287 | case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: |
288 | case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: |
289 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: |
290 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: |
291 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: |
292 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: |
293 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: |
294 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: |
295 | case HWTSTAMP_FILTER_PTP_V2_EVENT: |
296 | case HWTSTAMP_FILTER_PTP_V2_SYNC: |
297 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: |
298 | case HWTSTAMP_FILTER_NTP_ALL: |
299 | port->ptp_rx_cmd = true; |
300 | cfg->rx_filter = HWTSTAMP_FILTER_ALL; |
301 | break; |
302 | default: |
303 | return -ERANGE; |
304 | } |
305 | |
306 | /* Commit back the result & save it */ |
307 | mutex_lock(&lan966x->ptp_lock); |
308 | phc = &lan966x->phc[LAN966X_PHC_PORT]; |
309 | phc->hwtstamp_config = *cfg; |
310 | mutex_unlock(lock: &lan966x->ptp_lock); |
311 | |
312 | return 0; |
313 | } |
314 | |
315 | void lan966x_ptp_hwtstamp_get(struct lan966x_port *port, |
316 | struct kernel_hwtstamp_config *cfg) |
317 | { |
318 | struct lan966x *lan966x = port->lan966x; |
319 | struct lan966x_phc *phc; |
320 | |
321 | phc = &lan966x->phc[LAN966X_PHC_PORT]; |
322 | *cfg = phc->hwtstamp_config; |
323 | } |
324 | |
325 | static int lan966x_ptp_classify(struct lan966x_port *port, struct sk_buff *skb) |
326 | { |
327 | struct ptp_header *; |
328 | u8 msgtype; |
329 | int type; |
330 | |
331 | if (port->ptp_tx_cmd == IFH_REW_OP_NOOP) |
332 | return IFH_REW_OP_NOOP; |
333 | |
334 | type = ptp_classify_raw(skb); |
335 | if (type == PTP_CLASS_NONE) |
336 | return IFH_REW_OP_NOOP; |
337 | |
338 | header = ptp_parse_header(skb, type); |
339 | if (!header) |
340 | return IFH_REW_OP_NOOP; |
341 | |
342 | if (port->ptp_tx_cmd == IFH_REW_OP_TWO_STEP_PTP) |
343 | return IFH_REW_OP_TWO_STEP_PTP; |
344 | |
345 | /* If it is sync and run 1 step then set the correct operation, |
346 | * otherwise run as 2 step |
347 | */ |
348 | msgtype = ptp_get_msgtype(hdr: header, type); |
349 | if ((msgtype & 0xf) == 0) |
350 | return IFH_REW_OP_ONE_STEP_PTP; |
351 | |
352 | return IFH_REW_OP_TWO_STEP_PTP; |
353 | } |
354 | |
355 | static void lan966x_ptp_txtstamp_old_release(struct lan966x_port *port) |
356 | { |
357 | struct sk_buff *skb, *skb_tmp; |
358 | unsigned long flags; |
359 | |
360 | spin_lock_irqsave(&port->tx_skbs.lock, flags); |
361 | skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { |
362 | if time_after(LAN966X_SKB_CB(skb)->jiffies + LAN966X_PTP_TIMEOUT, |
363 | jiffies) |
364 | break; |
365 | |
366 | __skb_unlink(skb, list: &port->tx_skbs); |
367 | dev_kfree_skb_any(skb); |
368 | } |
369 | spin_unlock_irqrestore(lock: &port->tx_skbs.lock, flags); |
370 | } |
371 | |
372 | int lan966x_ptp_txtstamp_request(struct lan966x_port *port, |
373 | struct sk_buff *skb) |
374 | { |
375 | struct lan966x *lan966x = port->lan966x; |
376 | unsigned long flags; |
377 | u8 rew_op; |
378 | |
379 | rew_op = lan966x_ptp_classify(port, skb); |
380 | LAN966X_SKB_CB(skb)->rew_op = rew_op; |
381 | |
382 | if (rew_op != IFH_REW_OP_TWO_STEP_PTP) |
383 | return 0; |
384 | |
385 | lan966x_ptp_txtstamp_old_release(port); |
386 | |
387 | spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); |
388 | if (lan966x->ptp_skbs == LAN966X_MAX_PTP_ID) { |
389 | spin_unlock_irqrestore(lock: &lan966x->ptp_ts_id_lock, flags); |
390 | return -EBUSY; |
391 | } |
392 | |
393 | skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; |
394 | |
395 | skb_queue_tail(list: &port->tx_skbs, newsk: skb); |
396 | LAN966X_SKB_CB(skb)->ts_id = port->ts_id; |
397 | LAN966X_SKB_CB(skb)->jiffies = jiffies; |
398 | |
399 | lan966x->ptp_skbs++; |
400 | port->ts_id++; |
401 | if (port->ts_id == LAN966X_MAX_PTP_ID) |
402 | port->ts_id = 0; |
403 | |
404 | spin_unlock_irqrestore(lock: &lan966x->ptp_ts_id_lock, flags); |
405 | |
406 | return 0; |
407 | } |
408 | |
409 | void lan966x_ptp_txtstamp_release(struct lan966x_port *port, |
410 | struct sk_buff *skb) |
411 | { |
412 | struct lan966x *lan966x = port->lan966x; |
413 | unsigned long flags; |
414 | |
415 | spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); |
416 | port->ts_id--; |
417 | lan966x->ptp_skbs--; |
418 | skb_unlink(skb, list: &port->tx_skbs); |
419 | spin_unlock_irqrestore(lock: &lan966x->ptp_ts_id_lock, flags); |
420 | } |
421 | |
422 | static void lan966x_get_hwtimestamp(struct lan966x *lan966x, |
423 | struct timespec64 *ts, |
424 | u32 nsec) |
425 | { |
426 | /* Read current PTP time to get seconds */ |
427 | unsigned long flags; |
428 | u32 curr_nsec; |
429 | |
430 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
431 | |
432 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | |
433 | PTP_PIN_CFG_PIN_DOM_SET(LAN966X_PHC_PORT) | |
434 | PTP_PIN_CFG_PIN_SYNC_SET(0), |
435 | PTP_PIN_CFG_PIN_ACTION | |
436 | PTP_PIN_CFG_PIN_DOM | |
437 | PTP_PIN_CFG_PIN_SYNC, |
438 | lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); |
439 | |
440 | ts->tv_sec = lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); |
441 | curr_nsec = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); |
442 | |
443 | ts->tv_nsec = nsec; |
444 | |
445 | /* Sec has incremented since the ts was registered */ |
446 | if (curr_nsec < nsec) |
447 | ts->tv_sec--; |
448 | |
449 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
450 | } |
451 | |
452 | irqreturn_t lan966x_ptp_irq_handler(int irq, void *args) |
453 | { |
454 | int budget = LAN966X_MAX_PTP_ID; |
455 | struct lan966x *lan966x = args; |
456 | |
457 | while (budget--) { |
458 | struct sk_buff *skb, *skb_tmp, *skb_match = NULL; |
459 | struct skb_shared_hwtstamps shhwtstamps; |
460 | struct lan966x_port *port; |
461 | struct timespec64 ts; |
462 | unsigned long flags; |
463 | u32 val, id, txport; |
464 | u32 delay; |
465 | |
466 | val = lan_rd(lan966x, PTP_TWOSTEP_CTRL); |
467 | |
468 | /* Check if a timestamp can be retrieved */ |
469 | if (!(val & PTP_TWOSTEP_CTRL_VLD)) |
470 | break; |
471 | |
472 | WARN_ON(val & PTP_TWOSTEP_CTRL_OVFL); |
473 | |
474 | if (!(val & PTP_TWOSTEP_CTRL_STAMP_TX)) |
475 | continue; |
476 | |
477 | /* Retrieve the ts Tx port */ |
478 | txport = PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val); |
479 | |
480 | /* Retrieve its associated skb */ |
481 | port = lan966x->ports[txport]; |
482 | |
483 | /* Retrieve the delay */ |
484 | delay = lan_rd(lan966x, PTP_TWOSTEP_STAMP); |
485 | delay = PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay); |
486 | |
487 | /* Get next timestamp from fifo, which needs to be the |
488 | * rx timestamp which represents the id of the frame |
489 | */ |
490 | lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1), |
491 | PTP_TWOSTEP_CTRL_NXT, |
492 | lan966x, PTP_TWOSTEP_CTRL); |
493 | |
494 | val = lan_rd(lan966x, PTP_TWOSTEP_CTRL); |
495 | |
496 | /* Check if a timestamp can be retried */ |
497 | if (!(val & PTP_TWOSTEP_CTRL_VLD)) |
498 | break; |
499 | |
500 | /* Read RX timestamping to get the ID */ |
501 | id = lan_rd(lan966x, PTP_TWOSTEP_STAMP); |
502 | |
503 | spin_lock_irqsave(&port->tx_skbs.lock, flags); |
504 | skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { |
505 | if (LAN966X_SKB_CB(skb)->ts_id != id) |
506 | continue; |
507 | |
508 | __skb_unlink(skb, list: &port->tx_skbs); |
509 | skb_match = skb; |
510 | break; |
511 | } |
512 | spin_unlock_irqrestore(lock: &port->tx_skbs.lock, flags); |
513 | |
514 | /* Next ts */ |
515 | lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1), |
516 | PTP_TWOSTEP_CTRL_NXT, |
517 | lan966x, PTP_TWOSTEP_CTRL); |
518 | |
519 | if (WARN_ON(!skb_match)) |
520 | continue; |
521 | |
522 | spin_lock_irqsave(&lan966x->ptp_ts_id_lock, flags); |
523 | lan966x->ptp_skbs--; |
524 | spin_unlock_irqrestore(lock: &lan966x->ptp_ts_id_lock, flags); |
525 | |
526 | /* Get the h/w timestamp */ |
527 | lan966x_get_hwtimestamp(lan966x, ts: &ts, nsec: delay); |
528 | |
529 | /* Set the timestamp into the skb */ |
530 | shhwtstamps.hwtstamp = ktime_set(secs: ts.tv_sec, nsecs: ts.tv_nsec); |
531 | skb_tstamp_tx(orig_skb: skb_match, hwtstamps: &shhwtstamps); |
532 | |
533 | dev_kfree_skb_any(skb: skb_match); |
534 | } |
535 | |
536 | return IRQ_HANDLED; |
537 | } |
538 | |
539 | irqreturn_t lan966x_ptp_ext_irq_handler(int irq, void *args) |
540 | { |
541 | struct lan966x *lan966x = args; |
542 | struct lan966x_phc *phc; |
543 | unsigned long flags; |
544 | u64 time = 0; |
545 | time64_t s; |
546 | int pin, i; |
547 | s64 ns; |
548 | |
549 | if (!(lan_rd(lan966x, PTP_PIN_INTR))) |
550 | return IRQ_NONE; |
551 | |
552 | /* Go through all domains and see which pin generated the interrupt */ |
553 | for (i = 0; i < LAN966X_PHC_COUNT; ++i) { |
554 | struct ptp_clock_event ptp_event = {0}; |
555 | |
556 | phc = &lan966x->phc[i]; |
557 | pin = ptp_find_pin_unlocked(ptp: phc->clock, func: PTP_PF_EXTTS, chan: 0); |
558 | if (pin == -1) |
559 | continue; |
560 | |
561 | if (!(lan_rd(lan966x, PTP_PIN_INTR) & BIT(pin))) |
562 | continue; |
563 | |
564 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
565 | |
566 | /* Enable to get the new interrupt. |
567 | * By writing 1 it clears the bit |
568 | */ |
569 | lan_wr(BIT(pin), lan966x, PTP_PIN_INTR); |
570 | |
571 | /* Get current time */ |
572 | s = lan_rd(lan966x, PTP_TOD_SEC_MSB(pin)); |
573 | s <<= 32; |
574 | s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(pin)); |
575 | ns = lan_rd(lan966x, PTP_TOD_NSEC(pin)); |
576 | ns &= PTP_TOD_NSEC_TOD_NSEC; |
577 | |
578 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
579 | |
580 | if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { |
581 | s--; |
582 | ns &= 0xf; |
583 | ns += 999999984; |
584 | } |
585 | time = ktime_set(secs: s, nsecs: ns); |
586 | |
587 | ptp_event.index = pin; |
588 | ptp_event.timestamp = time; |
589 | ptp_event.type = PTP_CLOCK_EXTTS; |
590 | ptp_clock_event(ptp: phc->clock, event: &ptp_event); |
591 | } |
592 | |
593 | return IRQ_HANDLED; |
594 | } |
595 | |
596 | static int lan966x_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) |
597 | { |
598 | struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); |
599 | struct lan966x *lan966x = phc->lan966x; |
600 | unsigned long flags; |
601 | bool neg_adj = 0; |
602 | u64 tod_inc; |
603 | u64 ref; |
604 | |
605 | if (!scaled_ppm) |
606 | return 0; |
607 | |
608 | if (scaled_ppm < 0) { |
609 | neg_adj = 1; |
610 | scaled_ppm = -scaled_ppm; |
611 | } |
612 | |
613 | tod_inc = lan966x_ptp_get_nominal_value(); |
614 | |
615 | /* The multiplication is split in 2 separate additions because of |
616 | * overflow issues. If scaled_ppm with 16bit fractional part was bigger |
617 | * than 20ppm then we got overflow. |
618 | */ |
619 | ref = LAN966X_1PPM_FORMAT * (scaled_ppm >> 16); |
620 | ref += (LAN966X_1PPM_FORMAT * (0xffff & scaled_ppm)) >> 16; |
621 | tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref; |
622 | |
623 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
624 | |
625 | lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(1 << BIT(phc->index)), |
626 | PTP_DOM_CFG_CLKCFG_DIS, |
627 | lan966x, PTP_DOM_CFG); |
628 | |
629 | lan_wr(val: (u32)tod_inc & 0xFFFFFFFF, lan966x, |
630 | PTP_CLK_PER_CFG(phc->index, 0)); |
631 | lan_wr(val: (u32)(tod_inc >> 32), lan966x, |
632 | PTP_CLK_PER_CFG(phc->index, 1)); |
633 | |
634 | lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), |
635 | PTP_DOM_CFG_CLKCFG_DIS, |
636 | lan966x, PTP_DOM_CFG); |
637 | |
638 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
639 | |
640 | return 0; |
641 | } |
642 | |
643 | static int lan966x_ptp_settime64(struct ptp_clock_info *ptp, |
644 | const struct timespec64 *ts) |
645 | { |
646 | struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); |
647 | struct lan966x *lan966x = phc->lan966x; |
648 | unsigned long flags; |
649 | |
650 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
651 | |
652 | /* Must be in IDLE mode before the time can be loaded */ |
653 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | |
654 | PTP_PIN_CFG_PIN_DOM_SET(phc->index) | |
655 | PTP_PIN_CFG_PIN_SYNC_SET(0), |
656 | PTP_PIN_CFG_PIN_ACTION | |
657 | PTP_PIN_CFG_PIN_DOM | |
658 | PTP_PIN_CFG_PIN_SYNC, |
659 | lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); |
660 | |
661 | /* Set new value */ |
662 | lan_wr(PTP_TOD_SEC_MSB_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)), |
663 | lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); |
664 | lan_wr(lower_32_bits(ts->tv_sec), |
665 | lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); |
666 | lan_wr(val: ts->tv_nsec, lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); |
667 | |
668 | /* Apply new values */ |
669 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) | |
670 | PTP_PIN_CFG_PIN_DOM_SET(phc->index) | |
671 | PTP_PIN_CFG_PIN_SYNC_SET(0), |
672 | PTP_PIN_CFG_PIN_ACTION | |
673 | PTP_PIN_CFG_PIN_DOM | |
674 | PTP_PIN_CFG_PIN_SYNC, |
675 | lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); |
676 | |
677 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
678 | |
679 | return 0; |
680 | } |
681 | |
682 | int lan966x_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts) |
683 | { |
684 | struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); |
685 | struct lan966x *lan966x = phc->lan966x; |
686 | unsigned long flags; |
687 | time64_t s; |
688 | s64 ns; |
689 | |
690 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
691 | |
692 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | |
693 | PTP_PIN_CFG_PIN_DOM_SET(phc->index) | |
694 | PTP_PIN_CFG_PIN_SYNC_SET(0), |
695 | PTP_PIN_CFG_PIN_ACTION | |
696 | PTP_PIN_CFG_PIN_DOM | |
697 | PTP_PIN_CFG_PIN_SYNC, |
698 | lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); |
699 | |
700 | s = lan_rd(lan966x, PTP_TOD_SEC_MSB(TOD_ACC_PIN)); |
701 | s <<= 32; |
702 | s |= lan_rd(lan966x, PTP_TOD_SEC_LSB(TOD_ACC_PIN)); |
703 | ns = lan_rd(lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); |
704 | ns &= PTP_TOD_NSEC_TOD_NSEC; |
705 | |
706 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
707 | |
708 | /* Deal with negative values */ |
709 | if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { |
710 | s--; |
711 | ns &= 0xf; |
712 | ns += 999999984; |
713 | } |
714 | |
715 | set_normalized_timespec64(ts, sec: s, nsec: ns); |
716 | return 0; |
717 | } |
718 | |
719 | static int lan966x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) |
720 | { |
721 | struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); |
722 | struct lan966x *lan966x = phc->lan966x; |
723 | |
724 | if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) { |
725 | unsigned long flags; |
726 | |
727 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
728 | |
729 | /* Must be in IDLE mode before the time can be loaded */ |
730 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | |
731 | PTP_PIN_CFG_PIN_DOM_SET(phc->index) | |
732 | PTP_PIN_CFG_PIN_SYNC_SET(0), |
733 | PTP_PIN_CFG_PIN_ACTION | |
734 | PTP_PIN_CFG_PIN_DOM | |
735 | PTP_PIN_CFG_PIN_SYNC, |
736 | lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); |
737 | |
738 | lan_wr(PTP_TOD_NSEC_TOD_NSEC_SET(delta), |
739 | lan966x, PTP_TOD_NSEC(TOD_ACC_PIN)); |
740 | |
741 | /* Adjust time with the value of PTP_TOD_NSEC */ |
742 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) | |
743 | PTP_PIN_CFG_PIN_DOM_SET(phc->index) | |
744 | PTP_PIN_CFG_PIN_SYNC_SET(0), |
745 | PTP_PIN_CFG_PIN_ACTION | |
746 | PTP_PIN_CFG_PIN_DOM | |
747 | PTP_PIN_CFG_PIN_SYNC, |
748 | lan966x, PTP_PIN_CFG(TOD_ACC_PIN)); |
749 | |
750 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
751 | } else { |
752 | /* Fall back using lan966x_ptp_settime64 which is not exact */ |
753 | struct timespec64 ts; |
754 | u64 now; |
755 | |
756 | lan966x_ptp_gettime64(ptp, ts: &ts); |
757 | |
758 | now = ktime_to_ns(kt: timespec64_to_ktime(ts)); |
759 | ts = ns_to_timespec64(nsec: now + delta); |
760 | |
761 | lan966x_ptp_settime64(ptp, ts: &ts); |
762 | } |
763 | |
764 | return 0; |
765 | } |
766 | |
767 | static int lan966x_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin, |
768 | enum ptp_pin_function func, unsigned int chan) |
769 | { |
770 | struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); |
771 | struct lan966x *lan966x = phc->lan966x; |
772 | struct ptp_clock_info *info; |
773 | int i; |
774 | |
775 | /* Currently support only 1 channel */ |
776 | if (chan != 0) |
777 | return -1; |
778 | |
779 | switch (func) { |
780 | case PTP_PF_NONE: |
781 | case PTP_PF_PEROUT: |
782 | case PTP_PF_EXTTS: |
783 | break; |
784 | default: |
785 | return -1; |
786 | } |
787 | |
788 | /* The PTP pins are shared by all the PHC. So it is required to see if |
789 | * the pin is connected to another PHC. The pin is connected to another |
790 | * PHC if that pin already has a function on that PHC. |
791 | */ |
792 | for (i = 0; i < LAN966X_PHC_COUNT; ++i) { |
793 | info = &lan966x->phc[i].info; |
794 | |
795 | /* Ignore the check with ourself */ |
796 | if (ptp == info) |
797 | continue; |
798 | |
799 | if (info->pin_config[pin].func == PTP_PF_PEROUT || |
800 | info->pin_config[pin].func == PTP_PF_EXTTS) |
801 | return -1; |
802 | } |
803 | |
804 | return 0; |
805 | } |
806 | |
807 | static int lan966x_ptp_perout(struct ptp_clock_info *ptp, |
808 | struct ptp_clock_request *rq, int on) |
809 | { |
810 | struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); |
811 | struct lan966x *lan966x = phc->lan966x; |
812 | struct timespec64 ts_phase, ts_period; |
813 | unsigned long flags; |
814 | s64 wf_high, wf_low; |
815 | bool pps = false; |
816 | int pin; |
817 | |
818 | if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE | |
819 | PTP_PEROUT_PHASE)) |
820 | return -EOPNOTSUPP; |
821 | |
822 | pin = ptp_find_pin(ptp: phc->clock, func: PTP_PF_PEROUT, chan: rq->perout.index); |
823 | if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) |
824 | return -EINVAL; |
825 | |
826 | if (!on) { |
827 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
828 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | |
829 | PTP_PIN_CFG_PIN_DOM_SET(phc->index) | |
830 | PTP_PIN_CFG_PIN_SYNC_SET(0), |
831 | PTP_PIN_CFG_PIN_ACTION | |
832 | PTP_PIN_CFG_PIN_DOM | |
833 | PTP_PIN_CFG_PIN_SYNC, |
834 | lan966x, PTP_PIN_CFG(pin)); |
835 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
836 | return 0; |
837 | } |
838 | |
839 | if (rq->perout.period.sec == 1 && |
840 | rq->perout.period.nsec == 0) |
841 | pps = true; |
842 | |
843 | if (rq->perout.flags & PTP_PEROUT_PHASE) { |
844 | ts_phase.tv_sec = rq->perout.phase.sec; |
845 | ts_phase.tv_nsec = rq->perout.phase.nsec; |
846 | } else { |
847 | ts_phase.tv_sec = rq->perout.start.sec; |
848 | ts_phase.tv_nsec = rq->perout.start.nsec; |
849 | } |
850 | |
851 | if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) { |
852 | dev_warn(lan966x->dev, |
853 | "Absolute time not supported!\n" ); |
854 | return -EINVAL; |
855 | } |
856 | |
857 | if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) { |
858 | struct timespec64 ts_on; |
859 | |
860 | ts_on.tv_sec = rq->perout.on.sec; |
861 | ts_on.tv_nsec = rq->perout.on.nsec; |
862 | |
863 | wf_high = timespec64_to_ns(ts: &ts_on); |
864 | } else { |
865 | wf_high = 5000; |
866 | } |
867 | |
868 | if (pps) { |
869 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
870 | lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(ts_phase.tv_nsec), |
871 | lan966x, PTP_WF_LOW_PERIOD(pin)); |
872 | lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), |
873 | lan966x, PTP_WF_HIGH_PERIOD(pin)); |
874 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | |
875 | PTP_PIN_CFG_PIN_DOM_SET(phc->index) | |
876 | PTP_PIN_CFG_PIN_SYNC_SET(3), |
877 | PTP_PIN_CFG_PIN_ACTION | |
878 | PTP_PIN_CFG_PIN_DOM | |
879 | PTP_PIN_CFG_PIN_SYNC, |
880 | lan966x, PTP_PIN_CFG(pin)); |
881 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
882 | return 0; |
883 | } |
884 | |
885 | ts_period.tv_sec = rq->perout.period.sec; |
886 | ts_period.tv_nsec = rq->perout.period.nsec; |
887 | |
888 | wf_low = timespec64_to_ns(ts: &ts_period); |
889 | wf_low -= wf_high; |
890 | |
891 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
892 | lan_wr(PTP_WF_LOW_PERIOD_PIN_WFL(wf_low), |
893 | lan966x, PTP_WF_LOW_PERIOD(pin)); |
894 | lan_wr(PTP_WF_HIGH_PERIOD_PIN_WFH(wf_high), |
895 | lan966x, PTP_WF_HIGH_PERIOD(pin)); |
896 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_CLOCK) | |
897 | PTP_PIN_CFG_PIN_DOM_SET(phc->index) | |
898 | PTP_PIN_CFG_PIN_SYNC_SET(0), |
899 | PTP_PIN_CFG_PIN_ACTION | |
900 | PTP_PIN_CFG_PIN_DOM | |
901 | PTP_PIN_CFG_PIN_SYNC, |
902 | lan966x, PTP_PIN_CFG(pin)); |
903 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
904 | |
905 | return 0; |
906 | } |
907 | |
908 | static int lan966x_ptp_extts(struct ptp_clock_info *ptp, |
909 | struct ptp_clock_request *rq, int on) |
910 | { |
911 | struct lan966x_phc *phc = container_of(ptp, struct lan966x_phc, info); |
912 | struct lan966x *lan966x = phc->lan966x; |
913 | unsigned long flags; |
914 | int pin; |
915 | u32 val; |
916 | |
917 | if (lan966x->ptp_ext_irq <= 0) |
918 | return -EOPNOTSUPP; |
919 | |
920 | /* Reject requests with unsupported flags */ |
921 | if (rq->extts.flags & ~(PTP_ENABLE_FEATURE | |
922 | PTP_RISING_EDGE | |
923 | PTP_STRICT_FLAGS)) |
924 | return -EOPNOTSUPP; |
925 | |
926 | pin = ptp_find_pin(ptp: phc->clock, func: PTP_PF_EXTTS, chan: rq->extts.index); |
927 | if (pin == -1 || pin >= LAN966X_PHC_PINS_NUM) |
928 | return -EINVAL; |
929 | |
930 | spin_lock_irqsave(&lan966x->ptp_clock_lock, flags); |
931 | lan_rmw(PTP_PIN_CFG_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | |
932 | PTP_PIN_CFG_PIN_SYNC_SET(on ? 3 : 0) | |
933 | PTP_PIN_CFG_PIN_DOM_SET(phc->index) | |
934 | PTP_PIN_CFG_PIN_SELECT_SET(pin), |
935 | PTP_PIN_CFG_PIN_ACTION | |
936 | PTP_PIN_CFG_PIN_SYNC | |
937 | PTP_PIN_CFG_PIN_DOM | |
938 | PTP_PIN_CFG_PIN_SELECT, |
939 | lan966x, PTP_PIN_CFG(pin)); |
940 | |
941 | val = lan_rd(lan966x, PTP_PIN_INTR_ENA); |
942 | if (on) |
943 | val |= BIT(pin); |
944 | else |
945 | val &= ~BIT(pin); |
946 | lan_wr(val, lan966x, PTP_PIN_INTR_ENA); |
947 | |
948 | spin_unlock_irqrestore(lock: &lan966x->ptp_clock_lock, flags); |
949 | |
950 | return 0; |
951 | } |
952 | |
953 | static int lan966x_ptp_enable(struct ptp_clock_info *ptp, |
954 | struct ptp_clock_request *rq, int on) |
955 | { |
956 | switch (rq->type) { |
957 | case PTP_CLK_REQ_PEROUT: |
958 | return lan966x_ptp_perout(ptp, rq, on); |
959 | case PTP_CLK_REQ_EXTTS: |
960 | return lan966x_ptp_extts(ptp, rq, on); |
961 | default: |
962 | return -EOPNOTSUPP; |
963 | } |
964 | |
965 | return 0; |
966 | } |
967 | |
968 | static struct ptp_clock_info lan966x_ptp_clock_info = { |
969 | .owner = THIS_MODULE, |
970 | .name = "lan966x ptp" , |
971 | .max_adj = 200000, |
972 | .gettime64 = lan966x_ptp_gettime64, |
973 | .settime64 = lan966x_ptp_settime64, |
974 | .adjtime = lan966x_ptp_adjtime, |
975 | .adjfine = lan966x_ptp_adjfine, |
976 | .verify = lan966x_ptp_verify, |
977 | .enable = lan966x_ptp_enable, |
978 | .n_per_out = LAN966X_PHC_PINS_NUM, |
979 | .n_ext_ts = LAN966X_PHC_PINS_NUM, |
980 | .n_pins = LAN966X_PHC_PINS_NUM, |
981 | }; |
982 | |
983 | static int lan966x_ptp_phc_init(struct lan966x *lan966x, |
984 | int index, |
985 | struct ptp_clock_info *clock_info) |
986 | { |
987 | struct lan966x_phc *phc = &lan966x->phc[index]; |
988 | struct ptp_pin_desc *p; |
989 | int i; |
990 | |
991 | for (i = 0; i < LAN966X_PHC_PINS_NUM; i++) { |
992 | p = &phc->pins[i]; |
993 | |
994 | snprintf(buf: p->name, size: sizeof(p->name), fmt: "pin%d" , i); |
995 | p->index = i; |
996 | p->func = PTP_PF_NONE; |
997 | } |
998 | |
999 | phc->info = *clock_info; |
1000 | phc->info.pin_config = &phc->pins[0]; |
1001 | phc->clock = ptp_clock_register(info: &phc->info, parent: lan966x->dev); |
1002 | if (IS_ERR(ptr: phc->clock)) |
1003 | return PTR_ERR(ptr: phc->clock); |
1004 | |
1005 | phc->index = index; |
1006 | phc->lan966x = lan966x; |
1007 | |
1008 | return 0; |
1009 | } |
1010 | |
1011 | int lan966x_ptp_init(struct lan966x *lan966x) |
1012 | { |
1013 | u64 tod_adj = lan966x_ptp_get_nominal_value(); |
1014 | struct lan966x_port *port; |
1015 | int err, i; |
1016 | |
1017 | if (!lan966x->ptp) |
1018 | return 0; |
1019 | |
1020 | for (i = 0; i < LAN966X_PHC_COUNT; ++i) { |
1021 | err = lan966x_ptp_phc_init(lan966x, index: i, clock_info: &lan966x_ptp_clock_info); |
1022 | if (err) |
1023 | return err; |
1024 | } |
1025 | |
1026 | spin_lock_init(&lan966x->ptp_clock_lock); |
1027 | spin_lock_init(&lan966x->ptp_ts_id_lock); |
1028 | mutex_init(&lan966x->ptp_lock); |
1029 | |
1030 | /* Disable master counters */ |
1031 | lan_wr(PTP_DOM_CFG_ENA_SET(0), lan966x, PTP_DOM_CFG); |
1032 | |
1033 | /* Configure the nominal TOD increment per clock cycle */ |
1034 | lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0x7), |
1035 | PTP_DOM_CFG_CLKCFG_DIS, |
1036 | lan966x, PTP_DOM_CFG); |
1037 | |
1038 | for (i = 0; i < LAN966X_PHC_COUNT; ++i) { |
1039 | lan_wr(val: (u32)tod_adj & 0xFFFFFFFF, lan966x, |
1040 | PTP_CLK_PER_CFG(i, 0)); |
1041 | lan_wr(val: (u32)(tod_adj >> 32), lan966x, |
1042 | PTP_CLK_PER_CFG(i, 1)); |
1043 | } |
1044 | |
1045 | lan_rmw(PTP_DOM_CFG_CLKCFG_DIS_SET(0), |
1046 | PTP_DOM_CFG_CLKCFG_DIS, |
1047 | lan966x, PTP_DOM_CFG); |
1048 | |
1049 | /* Enable master counters */ |
1050 | lan_wr(PTP_DOM_CFG_ENA_SET(0x7), lan966x, PTP_DOM_CFG); |
1051 | |
1052 | for (i = 0; i < lan966x->num_phys_ports; i++) { |
1053 | port = lan966x->ports[i]; |
1054 | if (!port) |
1055 | continue; |
1056 | |
1057 | skb_queue_head_init(list: &port->tx_skbs); |
1058 | } |
1059 | |
1060 | return 0; |
1061 | } |
1062 | |
1063 | void lan966x_ptp_deinit(struct lan966x *lan966x) |
1064 | { |
1065 | struct lan966x_port *port; |
1066 | int i; |
1067 | |
1068 | if (!lan966x->ptp) |
1069 | return; |
1070 | |
1071 | for (i = 0; i < lan966x->num_phys_ports; i++) { |
1072 | port = lan966x->ports[i]; |
1073 | if (!port) |
1074 | continue; |
1075 | |
1076 | skb_queue_purge(list: &port->tx_skbs); |
1077 | } |
1078 | |
1079 | for (i = 0; i < LAN966X_PHC_COUNT; ++i) |
1080 | ptp_clock_unregister(ptp: lan966x->phc[i].clock); |
1081 | } |
1082 | |
1083 | void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb, |
1084 | u64 src_port, u64 timestamp) |
1085 | { |
1086 | struct skb_shared_hwtstamps *shhwtstamps; |
1087 | struct lan966x_phc *phc; |
1088 | struct timespec64 ts; |
1089 | u64 full_ts_in_ns; |
1090 | |
1091 | if (!lan966x->ptp || |
1092 | !lan966x->ports[src_port]->ptp_rx_cmd) |
1093 | return; |
1094 | |
1095 | phc = &lan966x->phc[LAN966X_PHC_PORT]; |
1096 | lan966x_ptp_gettime64(ptp: &phc->info, ts: &ts); |
1097 | |
1098 | /* Drop the sub-ns precision */ |
1099 | timestamp = timestamp >> 2; |
1100 | if (ts.tv_nsec < timestamp) |
1101 | ts.tv_sec--; |
1102 | ts.tv_nsec = timestamp; |
1103 | full_ts_in_ns = ktime_set(secs: ts.tv_sec, nsecs: ts.tv_nsec); |
1104 | |
1105 | shhwtstamps = skb_hwtstamps(skb); |
1106 | shhwtstamps->hwtstamp = full_ts_in_ns; |
1107 | } |
1108 | |
1109 | u32 lan966x_ptp_get_period_ps(void) |
1110 | { |
1111 | /* This represents the system clock period in picoseconds */ |
1112 | return 15125; |
1113 | } |
1114 | |