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(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
 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(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
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, &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
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(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(&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(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(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
240void 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}