1// SPDX-License-Identifier: GPL-2.0
2/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
3
4#include "tsnep.h"
5
6void 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
22int 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
76static 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
104static 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
129static 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
156static 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
178static 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
205int 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
240void 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

source code of linux/drivers/net/ethernet/engleder/tsnep_ptp.c