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(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 
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 
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 
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 
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 
178 int tsnep_ptp_init(struct tsnep_adapter *adapter)
179 {
180 	int retval = 0;
181 
182 	adapter->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_NONE;
183 	adapter->hwtstamp_config.tx_type = HWTSTAMP_TX_OFF;
184 
185 	snprintf(adapter->ptp_clock_info.name, 16, "%s", TSNEP);
186 	adapter->ptp_clock_info.owner = THIS_MODULE;
187 	/* at most 2^-1ns adjustment every clock cycle for 8ns clock cycle time,
188 	 * stay slightly below because only bits below 2^-1ns are supported
189 	 */
190 	adapter->ptp_clock_info.max_adj = (500000000 / 8 - 1);
191 	adapter->ptp_clock_info.adjfine = tsnep_ptp_adjfine;
192 	adapter->ptp_clock_info.adjtime = tsnep_ptp_adjtime;
193 	adapter->ptp_clock_info.gettimex64 = tsnep_ptp_gettimex64;
194 	adapter->ptp_clock_info.settime64 = tsnep_ptp_settime64;
195 
196 	spin_lock_init(&adapter->ptp_lock);
197 
198 	adapter->ptp_clock = ptp_clock_register(&adapter->ptp_clock_info,
199 						&adapter->pdev->dev);
200 	if (IS_ERR(adapter->ptp_clock)) {
201 		netdev_err(adapter->netdev, "ptp_clock_register failed\n");
202 
203 		retval = PTR_ERR(adapter->ptp_clock);
204 		adapter->ptp_clock = NULL;
205 	} else if (adapter->ptp_clock) {
206 		netdev_info(adapter->netdev, "PHC added\n");
207 	}
208 
209 	return retval;
210 }
211 
212 void tsnep_ptp_cleanup(struct tsnep_adapter *adapter)
213 {
214 	if (adapter->ptp_clock) {
215 		ptp_clock_unregister(adapter->ptp_clock);
216 		netdev_info(adapter->netdev, "PHC removed\n");
217 	}
218 }
219