Commit | Line | Data |
---|---|---|
ef9814de EBE |
1 | /* |
2 | * Copyright (c) 2015, Mellanox Technologies. All rights reserved. | |
3 | * | |
4 | * This software is available to you under a choice of one of two | |
5 | * licenses. You may choose to be licensed under the terms of the GNU | |
6 | * General Public License (GPL) Version 2, available from the file | |
7 | * COPYING in the main directory of this source tree, or the | |
8 | * OpenIB.org BSD license below: | |
9 | * | |
10 | * Redistribution and use in source and binary forms, with or | |
11 | * without modification, are permitted provided that the following | |
12 | * conditions are met: | |
13 | * | |
14 | * - Redistributions of source code must retain the above | |
15 | * copyright notice, this list of conditions and the following | |
16 | * disclaimer. | |
17 | * | |
18 | * - Redistributions in binary form must reproduce the above | |
19 | * copyright notice, this list of conditions and the following | |
20 | * disclaimer in the documentation and/or other materials | |
21 | * provided with the distribution. | |
22 | * | |
23 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, | |
24 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF | |
25 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND | |
26 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS | |
27 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN | |
28 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN | |
29 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | |
30 | * SOFTWARE. | |
31 | */ | |
32 | ||
33 | #include <linux/clocksource.h> | |
34 | #include "en.h" | |
35 | ||
36 | enum { | |
37 | MLX5E_CYCLES_SHIFT = 23 | |
38 | }; | |
39 | ||
40 | void mlx5e_fill_hwstamp(struct mlx5e_tstamp *tstamp, u64 timestamp, | |
41 | struct skb_shared_hwtstamps *hwts) | |
42 | { | |
43 | u64 nsec; | |
44 | ||
45 | read_lock(&tstamp->lock); | |
46 | nsec = timecounter_cyc2time(&tstamp->clock, timestamp); | |
47 | read_unlock(&tstamp->lock); | |
48 | ||
49 | hwts->hwtstamp = ns_to_ktime(nsec); | |
50 | } | |
51 | ||
52 | static cycle_t mlx5e_read_internal_timer(const struct cyclecounter *cc) | |
53 | { | |
54 | struct mlx5e_tstamp *tstamp = container_of(cc, struct mlx5e_tstamp, | |
55 | cycles); | |
56 | ||
57 | return mlx5_read_internal_timer(tstamp->mdev) & cc->mask; | |
58 | } | |
59 | ||
60 | static void mlx5e_timestamp_overflow(struct work_struct *work) | |
61 | { | |
62 | struct delayed_work *dwork = to_delayed_work(work); | |
63 | struct mlx5e_tstamp *tstamp = container_of(dwork, struct mlx5e_tstamp, | |
64 | overflow_work); | |
65 | ||
66 | write_lock(&tstamp->lock); | |
67 | timecounter_read(&tstamp->clock); | |
68 | write_unlock(&tstamp->lock); | |
69 | schedule_delayed_work(&tstamp->overflow_work, tstamp->overflow_period); | |
70 | } | |
71 | ||
72 | int mlx5e_hwstamp_set(struct net_device *dev, struct ifreq *ifr) | |
73 | { | |
74 | struct mlx5e_priv *priv = netdev_priv(dev); | |
75 | struct hwtstamp_config config; | |
76 | ||
77 | if (!MLX5_CAP_GEN(priv->mdev, device_frequency_khz)) | |
78 | return -EOPNOTSUPP; | |
79 | ||
80 | if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) | |
81 | return -EFAULT; | |
82 | ||
83 | /* TX HW timestamp */ | |
84 | switch (config.tx_type) { | |
85 | case HWTSTAMP_TX_OFF: | |
86 | case HWTSTAMP_TX_ON: | |
87 | break; | |
88 | default: | |
89 | return -ERANGE; | |
90 | } | |
91 | ||
92 | /* RX HW timestamp */ | |
93 | switch (config.rx_filter) { | |
94 | case HWTSTAMP_FILTER_NONE: | |
95 | break; | |
96 | case HWTSTAMP_FILTER_ALL: | |
97 | case HWTSTAMP_FILTER_SOME: | |
98 | case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: | |
99 | case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: | |
100 | case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: | |
101 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: | |
102 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: | |
103 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: | |
104 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: | |
105 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: | |
106 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: | |
107 | case HWTSTAMP_FILTER_PTP_V2_EVENT: | |
108 | case HWTSTAMP_FILTER_PTP_V2_SYNC: | |
109 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: | |
110 | config.rx_filter = HWTSTAMP_FILTER_ALL; | |
111 | break; | |
112 | default: | |
113 | return -ERANGE; | |
114 | } | |
115 | ||
116 | memcpy(&priv->tstamp.hwtstamp_config, &config, sizeof(config)); | |
117 | ||
118 | return copy_to_user(ifr->ifr_data, &config, | |
119 | sizeof(config)) ? -EFAULT : 0; | |
120 | } | |
121 | ||
122 | int mlx5e_hwstamp_get(struct net_device *dev, struct ifreq *ifr) | |
123 | { | |
124 | struct mlx5e_priv *priv = netdev_priv(dev); | |
125 | struct hwtstamp_config *cfg = &priv->tstamp.hwtstamp_config; | |
126 | ||
127 | if (!MLX5_CAP_GEN(priv->mdev, device_frequency_khz)) | |
128 | return -EOPNOTSUPP; | |
129 | ||
130 | return copy_to_user(ifr->ifr_data, cfg, sizeof(*cfg)) ? -EFAULT : 0; | |
131 | } | |
132 | ||
3d8c38af EBE |
133 | static int mlx5e_ptp_settime(struct ptp_clock_info *ptp, |
134 | const struct timespec64 *ts) | |
135 | { | |
136 | struct mlx5e_tstamp *tstamp = container_of(ptp, struct mlx5e_tstamp, | |
137 | ptp_info); | |
138 | u64 ns = timespec64_to_ns(ts); | |
139 | ||
140 | write_lock(&tstamp->lock); | |
141 | timecounter_init(&tstamp->clock, &tstamp->cycles, ns); | |
142 | write_unlock(&tstamp->lock); | |
143 | ||
144 | return 0; | |
145 | } | |
146 | ||
147 | static int mlx5e_ptp_gettime(struct ptp_clock_info *ptp, | |
148 | struct timespec64 *ts) | |
149 | { | |
150 | struct mlx5e_tstamp *tstamp = container_of(ptp, struct mlx5e_tstamp, | |
151 | ptp_info); | |
152 | u64 ns; | |
153 | ||
154 | write_lock(&tstamp->lock); | |
155 | ns = timecounter_read(&tstamp->clock); | |
156 | write_unlock(&tstamp->lock); | |
157 | ||
158 | *ts = ns_to_timespec64(ns); | |
159 | ||
160 | return 0; | |
161 | } | |
162 | ||
163 | static int mlx5e_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) | |
164 | { | |
165 | struct mlx5e_tstamp *tstamp = container_of(ptp, struct mlx5e_tstamp, | |
166 | ptp_info); | |
167 | ||
168 | write_lock(&tstamp->lock); | |
169 | timecounter_adjtime(&tstamp->clock, delta); | |
170 | write_unlock(&tstamp->lock); | |
171 | ||
172 | return 0; | |
173 | } | |
174 | ||
175 | static int mlx5e_ptp_adjfreq(struct ptp_clock_info *ptp, s32 delta) | |
176 | { | |
177 | u64 adj; | |
178 | u32 diff; | |
179 | int neg_adj = 0; | |
180 | struct mlx5e_tstamp *tstamp = container_of(ptp, struct mlx5e_tstamp, | |
181 | ptp_info); | |
182 | ||
183 | if (delta < 0) { | |
184 | neg_adj = 1; | |
185 | delta = -delta; | |
186 | } | |
187 | ||
188 | adj = tstamp->nominal_c_mult; | |
189 | adj *= delta; | |
190 | diff = div_u64(adj, 1000000000ULL); | |
191 | ||
192 | write_lock(&tstamp->lock); | |
193 | timecounter_read(&tstamp->clock); | |
194 | tstamp->cycles.mult = neg_adj ? tstamp->nominal_c_mult - diff : | |
195 | tstamp->nominal_c_mult + diff; | |
196 | write_unlock(&tstamp->lock); | |
197 | ||
198 | return 0; | |
199 | } | |
200 | ||
201 | static const struct ptp_clock_info mlx5e_ptp_clock_info = { | |
202 | .owner = THIS_MODULE, | |
203 | .max_adj = 100000000, | |
204 | .n_alarm = 0, | |
205 | .n_ext_ts = 0, | |
206 | .n_per_out = 0, | |
207 | .n_pins = 0, | |
208 | .pps = 0, | |
209 | .adjfreq = mlx5e_ptp_adjfreq, | |
210 | .adjtime = mlx5e_ptp_adjtime, | |
211 | .gettime64 = mlx5e_ptp_gettime, | |
212 | .settime64 = mlx5e_ptp_settime, | |
213 | .enable = NULL, | |
214 | }; | |
215 | ||
ef9814de EBE |
216 | static void mlx5e_timestamp_init_config(struct mlx5e_tstamp *tstamp) |
217 | { | |
218 | tstamp->hwtstamp_config.tx_type = HWTSTAMP_TX_OFF; | |
219 | tstamp->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_NONE; | |
220 | } | |
221 | ||
222 | void mlx5e_timestamp_init(struct mlx5e_priv *priv) | |
223 | { | |
224 | struct mlx5e_tstamp *tstamp = &priv->tstamp; | |
225 | u64 ns; | |
226 | u64 frac = 0; | |
227 | u32 dev_freq; | |
228 | ||
229 | mlx5e_timestamp_init_config(tstamp); | |
230 | dev_freq = MLX5_CAP_GEN(priv->mdev, device_frequency_khz); | |
231 | if (!dev_freq) { | |
232 | mlx5_core_warn(priv->mdev, "invalid device_frequency_khz, aborting HW clock init\n"); | |
233 | return; | |
234 | } | |
235 | rwlock_init(&tstamp->lock); | |
236 | tstamp->cycles.read = mlx5e_read_internal_timer; | |
237 | tstamp->cycles.shift = MLX5E_CYCLES_SHIFT; | |
238 | tstamp->cycles.mult = clocksource_khz2mult(dev_freq, | |
239 | tstamp->cycles.shift); | |
240 | tstamp->nominal_c_mult = tstamp->cycles.mult; | |
241 | tstamp->cycles.mask = CLOCKSOURCE_MASK(41); | |
242 | tstamp->mdev = priv->mdev; | |
243 | ||
244 | timecounter_init(&tstamp->clock, &tstamp->cycles, | |
245 | ktime_to_ns(ktime_get_real())); | |
246 | ||
247 | /* Calculate period in seconds to call the overflow watchdog - to make | |
248 | * sure counter is checked at least once every wrap around. | |
249 | */ | |
250 | ns = cyclecounter_cyc2ns(&tstamp->cycles, tstamp->cycles.mask, | |
251 | frac, &frac); | |
252 | do_div(ns, NSEC_PER_SEC / 2 / HZ); | |
253 | tstamp->overflow_period = ns; | |
254 | ||
255 | INIT_DELAYED_WORK(&tstamp->overflow_work, mlx5e_timestamp_overflow); | |
256 | if (tstamp->overflow_period) | |
257 | schedule_delayed_work(&tstamp->overflow_work, 0); | |
258 | else | |
259 | mlx5_core_warn(priv->mdev, "invalid overflow period, overflow_work is not scheduled\n"); | |
3d8c38af EBE |
260 | |
261 | /* Configure the PHC */ | |
262 | tstamp->ptp_info = mlx5e_ptp_clock_info; | |
263 | snprintf(tstamp->ptp_info.name, 16, "mlx5 ptp"); | |
264 | ||
265 | tstamp->ptp = ptp_clock_register(&tstamp->ptp_info, | |
266 | &priv->mdev->pdev->dev); | |
267 | if (IS_ERR_OR_NULL(tstamp->ptp)) { | |
268 | mlx5_core_warn(priv->mdev, "ptp_clock_register failed %ld\n", | |
269 | PTR_ERR(tstamp->ptp)); | |
270 | tstamp->ptp = NULL; | |
271 | } | |
ef9814de EBE |
272 | } |
273 | ||
274 | void mlx5e_timestamp_cleanup(struct mlx5e_priv *priv) | |
275 | { | |
276 | struct mlx5e_tstamp *tstamp = &priv->tstamp; | |
277 | ||
278 | if (!MLX5_CAP_GEN(priv->mdev, device_frequency_khz)) | |
279 | return; | |
280 | ||
3d8c38af EBE |
281 | if (priv->tstamp.ptp) { |
282 | ptp_clock_unregister(priv->tstamp.ptp); | |
283 | priv->tstamp.ptp = NULL; | |
284 | } | |
285 | ||
ef9814de EBE |
286 | cancel_delayed_work_sync(&tstamp->overflow_work); |
287 | } |