1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */ |
3 | |
4 | #include "tsnep.h" |
5 | |
6 | void tsnep_get_system_time(struct tsnep_adapter *adapter, u64 *time) |
7 | { |
8 | u32 high_before; |
9 | u32 low; |
10 | u32 high; |
11 | |
12 | /* read high dword twice to detect overrun */ |
13 | high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH); |
14 | do { |
15 | low = ioread32(adapter->addr + ECM_SYSTEM_TIME_LOW); |
16 | high_before = high; |
17 | high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH); |
18 | } while (high != high_before); |
19 | *time = (((u64)high) << 32) | ((u64)low); |
20 | } |
21 | |
22 | int tsnep_ptp_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd) |
23 | { |
24 | struct tsnep_adapter *adapter = netdev_priv(dev: netdev); |
25 | struct hwtstamp_config config; |
26 | |
27 | if (!ifr) |
28 | return -EINVAL; |
29 | |
30 | if (cmd == SIOCSHWTSTAMP) { |
31 | if (copy_from_user(to: &config, from: ifr->ifr_data, n: sizeof(config))) |
32 | return -EFAULT; |
33 | |
34 | switch (config.tx_type) { |
35 | case HWTSTAMP_TX_OFF: |
36 | case HWTSTAMP_TX_ON: |
37 | break; |
38 | default: |
39 | return -ERANGE; |
40 | } |
41 | |
42 | switch (config.rx_filter) { |
43 | case HWTSTAMP_FILTER_NONE: |
44 | break; |
45 | case HWTSTAMP_FILTER_ALL: |
46 | case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: |
47 | case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: |
48 | case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: |
49 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: |
50 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: |
51 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: |
52 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: |
53 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: |
54 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: |
55 | case HWTSTAMP_FILTER_PTP_V2_EVENT: |
56 | case HWTSTAMP_FILTER_PTP_V2_SYNC: |
57 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: |
58 | case HWTSTAMP_FILTER_NTP_ALL: |
59 | config.rx_filter = HWTSTAMP_FILTER_ALL; |
60 | break; |
61 | default: |
62 | return -ERANGE; |
63 | } |
64 | |
65 | memcpy(&adapter->hwtstamp_config, &config, |
66 | sizeof(adapter->hwtstamp_config)); |
67 | } |
68 | |
69 | if (copy_to_user(to: ifr->ifr_data, from: &adapter->hwtstamp_config, |
70 | n: sizeof(adapter->hwtstamp_config))) |
71 | return -EFAULT; |
72 | |
73 | return 0; |
74 | } |
75 | |
76 | static int tsnep_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) |
77 | { |
78 | struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, |
79 | ptp_clock_info); |
80 | bool negative = false; |
81 | u64 rate_offset; |
82 | |
83 | if (scaled_ppm < 0) { |
84 | scaled_ppm = -scaled_ppm; |
85 | negative = true; |
86 | } |
87 | |
88 | /* convert from 16 bit to 32 bit binary fractional, divide by 1000000 to |
89 | * eliminate ppm, multiply with 8 to compensate 8ns clock cycle time, |
90 | * simplify calculation because 15625 * 8 = 1000000 / 8 |
91 | */ |
92 | rate_offset = scaled_ppm; |
93 | rate_offset <<= 16 - 3; |
94 | rate_offset = div_u64(dividend: rate_offset, divisor: 15625); |
95 | |
96 | rate_offset &= ECM_CLOCK_RATE_OFFSET_MASK; |
97 | if (negative) |
98 | rate_offset |= ECM_CLOCK_RATE_OFFSET_SIGN; |
99 | iowrite32(rate_offset & 0xFFFFFFFF, adapter->addr + ECM_CLOCK_RATE); |
100 | |
101 | return 0; |
102 | } |
103 | |
104 | static int tsnep_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) |
105 | { |
106 | struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, |
107 | ptp_clock_info); |
108 | u64 system_time; |
109 | unsigned long flags; |
110 | |
111 | spin_lock_irqsave(&adapter->ptp_lock, flags); |
112 | |
113 | tsnep_get_system_time(adapter, time: &system_time); |
114 | |
115 | system_time += delta; |
116 | |
117 | /* high dword is buffered in hardware and synchronously written to |
118 | * system time when low dword is written |
119 | */ |
120 | iowrite32(system_time >> 32, adapter->addr + ECM_SYSTEM_TIME_HIGH); |
121 | iowrite32(system_time & 0xFFFFFFFF, |
122 | adapter->addr + ECM_SYSTEM_TIME_LOW); |
123 | |
124 | spin_unlock_irqrestore(lock: &adapter->ptp_lock, flags); |
125 | |
126 | return 0; |
127 | } |
128 | |
129 | static int tsnep_ptp_gettimex64(struct ptp_clock_info *ptp, |
130 | struct timespec64 *ts, |
131 | struct ptp_system_timestamp *sts) |
132 | { |
133 | struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, |
134 | ptp_clock_info); |
135 | u32 high_before; |
136 | u32 low; |
137 | u32 high; |
138 | u64 system_time; |
139 | |
140 | /* read high dword twice to detect overrun */ |
141 | high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH); |
142 | do { |
143 | ptp_read_system_prets(sts); |
144 | low = ioread32(adapter->addr + ECM_SYSTEM_TIME_LOW); |
145 | ptp_read_system_postts(sts); |
146 | high_before = high; |
147 | high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH); |
148 | } while (high != high_before); |
149 | system_time = (((u64)high) << 32) | ((u64)low); |
150 | |
151 | *ts = ns_to_timespec64(nsec: system_time); |
152 | |
153 | return 0; |
154 | } |
155 | |
156 | static int tsnep_ptp_settime64(struct ptp_clock_info *ptp, |
157 | const struct timespec64 *ts) |
158 | { |
159 | struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, |
160 | ptp_clock_info); |
161 | u64 system_time = timespec64_to_ns(ts); |
162 | unsigned long flags; |
163 | |
164 | spin_lock_irqsave(&adapter->ptp_lock, flags); |
165 | |
166 | /* high dword is buffered in hardware and synchronously written to |
167 | * system time when low dword is written |
168 | */ |
169 | iowrite32(system_time >> 32, adapter->addr + ECM_SYSTEM_TIME_HIGH); |
170 | iowrite32(system_time & 0xFFFFFFFF, |
171 | adapter->addr + ECM_SYSTEM_TIME_LOW); |
172 | |
173 | spin_unlock_irqrestore(lock: &adapter->ptp_lock, flags); |
174 | |
175 | return 0; |
176 | } |
177 | |
178 | static int tsnep_ptp_getcyclesx64(struct ptp_clock_info *ptp, |
179 | struct timespec64 *ts, |
180 | struct ptp_system_timestamp *sts) |
181 | { |
182 | struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, |
183 | ptp_clock_info); |
184 | u32 high_before; |
185 | u32 low; |
186 | u32 high; |
187 | u64 counter; |
188 | |
189 | /* read high dword twice to detect overrun */ |
190 | high = ioread32(adapter->addr + ECM_COUNTER_HIGH); |
191 | do { |
192 | ptp_read_system_prets(sts); |
193 | low = ioread32(adapter->addr + ECM_COUNTER_LOW); |
194 | ptp_read_system_postts(sts); |
195 | high_before = high; |
196 | high = ioread32(adapter->addr + ECM_COUNTER_HIGH); |
197 | } while (high != high_before); |
198 | counter = (((u64)high) << 32) | ((u64)low); |
199 | |
200 | *ts = ns_to_timespec64(nsec: counter); |
201 | |
202 | return 0; |
203 | } |
204 | |
205 | int tsnep_ptp_init(struct tsnep_adapter *adapter) |
206 | { |
207 | int retval = 0; |
208 | |
209 | adapter->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_NONE; |
210 | adapter->hwtstamp_config.tx_type = HWTSTAMP_TX_OFF; |
211 | |
212 | snprintf(buf: adapter->ptp_clock_info.name, size: 16, fmt: "%s" , TSNEP); |
213 | adapter->ptp_clock_info.owner = THIS_MODULE; |
214 | /* at most 2^-1ns adjustment every clock cycle for 8ns clock cycle time, |
215 | * stay slightly below because only bits below 2^-1ns are supported |
216 | */ |
217 | adapter->ptp_clock_info.max_adj = (500000000 / 8 - 1); |
218 | adapter->ptp_clock_info.adjfine = tsnep_ptp_adjfine; |
219 | adapter->ptp_clock_info.adjtime = tsnep_ptp_adjtime; |
220 | adapter->ptp_clock_info.gettimex64 = tsnep_ptp_gettimex64; |
221 | adapter->ptp_clock_info.settime64 = tsnep_ptp_settime64; |
222 | adapter->ptp_clock_info.getcyclesx64 = tsnep_ptp_getcyclesx64; |
223 | |
224 | spin_lock_init(&adapter->ptp_lock); |
225 | |
226 | adapter->ptp_clock = ptp_clock_register(info: &adapter->ptp_clock_info, |
227 | parent: &adapter->pdev->dev); |
228 | if (IS_ERR(ptr: adapter->ptp_clock)) { |
229 | netdev_err(dev: adapter->netdev, format: "ptp_clock_register failed\n" ); |
230 | |
231 | retval = PTR_ERR(ptr: adapter->ptp_clock); |
232 | adapter->ptp_clock = NULL; |
233 | } else if (adapter->ptp_clock) { |
234 | netdev_info(dev: adapter->netdev, format: "PHC added\n" ); |
235 | } |
236 | |
237 | return retval; |
238 | } |
239 | |
240 | void tsnep_ptp_cleanup(struct tsnep_adapter *adapter) |
241 | { |
242 | if (adapter->ptp_clock) { |
243 | ptp_clock_unregister(ptp: adapter->ptp_clock); |
244 | netdev_info(dev: adapter->netdev, format: "PHC removed\n" ); |
245 | } |
246 | } |
247 | |