1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
3
4 #include "tsnep.h"
5
tsnep_get_system_time(struct tsnep_adapter * adapter,u64 * time)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
tsnep_ptp_ioctl(struct net_device * netdev,struct ifreq * ifr,int cmd)22 int tsnep_ptp_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
23 {
24 struct tsnep_adapter *adapter = netdev_priv(netdev);
25 struct hwtstamp_config config;
26
27 if (!ifr)
28 return -EINVAL;
29
30 if (cmd == SIOCSHWTSTAMP) {
31 if (copy_from_user(&config, ifr->ifr_data, 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(ifr->ifr_data, &adapter->hwtstamp_config,
70 sizeof(adapter->hwtstamp_config)))
71 return -EFAULT;
72
73 return 0;
74 }
75
tsnep_ptp_adjfine(struct ptp_clock_info * ptp,long scaled_ppm)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(rate_offset, 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
tsnep_ptp_adjtime(struct ptp_clock_info * ptp,s64 delta)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, &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(&adapter->ptp_lock, flags);
125
126 return 0;
127 }
128
tsnep_ptp_gettimex64(struct ptp_clock_info * ptp,struct timespec64 * ts,struct ptp_system_timestamp * sts)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(system_time);
152
153 return 0;
154 }
155
tsnep_ptp_settime64(struct ptp_clock_info * ptp,const struct timespec64 * ts)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(&adapter->ptp_lock, flags);
174
175 return 0;
176 }
177
tsnep_ptp_getcyclesx64(struct ptp_clock_info * ptp,struct timespec64 * ts,struct ptp_system_timestamp * sts)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(counter);
201
202 return 0;
203 }
204
tsnep_ptp_init(struct tsnep_adapter * adapter)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(adapter->ptp_clock_info.name, 16, "%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(&adapter->ptp_clock_info,
227 &adapter->pdev->dev);
228 if (IS_ERR(adapter->ptp_clock)) {
229 netdev_err(adapter->netdev, "ptp_clock_register failed\n");
230
231 retval = PTR_ERR(adapter->ptp_clock);
232 adapter->ptp_clock = NULL;
233 } else if (adapter->ptp_clock) {
234 netdev_info(adapter->netdev, "PHC added\n");
235 }
236
237 return retval;
238 }
239
tsnep_ptp_cleanup(struct tsnep_adapter * adapter)240 void tsnep_ptp_cleanup(struct tsnep_adapter *adapter)
241 {
242 if (adapter->ptp_clock) {
243 ptp_clock_unregister(adapter->ptp_clock);
244 netdev_info(adapter->netdev, "PHC removed\n");
245 }
246 }
247