tsnep_ptp.c

  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_hwtstamp_get(struct net_device *netdev,
 23			   struct kernel_hwtstamp_config *config)
 24{
 25	struct tsnep_adapter *adapter = netdev_priv(netdev);
 26
 27	*config = adapter->hwtstamp_config;
 28	return 0;
 29}
 30
 31int tsnep_ptp_hwtstamp_set(struct net_device *netdev,
 32			   struct kernel_hwtstamp_config *config,
 33			   struct netlink_ext_ack *extack)
 34{
 35	struct tsnep_adapter *adapter = netdev_priv(netdev);
 36
 37	switch (config->tx_type) {
 38	case HWTSTAMP_TX_OFF:
 39	case HWTSTAMP_TX_ON:
 40		break;
 41	default:
 42		return -ERANGE;
 43	}
 44
 45	switch (config->rx_filter) {
 46	case HWTSTAMP_FILTER_NONE:
 47		break;
 48	case HWTSTAMP_FILTER_ALL:
 49	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 50	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 51	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 52	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 53	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 54	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 55	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 56	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 57	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 58	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 59	case HWTSTAMP_FILTER_PTP_V2_SYNC:
 60	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 61	case HWTSTAMP_FILTER_NTP_ALL:
 62		config->rx_filter = HWTSTAMP_FILTER_ALL;
 63		break;
 64	default:
 65		return -ERANGE;
 66	}
 67
 68	adapter->hwtstamp_config = *config;
 69	return 0;
 70}
 71
 72static int tsnep_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 73{
 74	struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
 75						     ptp_clock_info);
 76	bool negative = false;
 77	u64 rate_offset;
 78
 79	if (scaled_ppm < 0) {
 80		scaled_ppm = -scaled_ppm;
 81		negative = true;
 82	}
 83
 84	/* convert from 16 bit to 32 bit binary fractional, divide by 1000000 to
 85	 * eliminate ppm, multiply with 8 to compensate 8ns clock cycle time,
 86	 * simplify calculation because 15625 * 8 = 1000000 / 8
 87	 */
 88	rate_offset = scaled_ppm;
 89	rate_offset <<= 16 - 3;
 90	rate_offset = div_u64(rate_offset, 15625);
 91
 92	rate_offset &= ECM_CLOCK_RATE_OFFSET_MASK;
 93	if (negative)
 94		rate_offset |= ECM_CLOCK_RATE_OFFSET_SIGN;
 95	iowrite32(rate_offset & 0xFFFFFFFF, adapter->addr + ECM_CLOCK_RATE);
 96
 97	return 0;
 98}
 99
100static int tsnep_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
101{
102	struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
103						     ptp_clock_info);
104	u64 system_time;
105	unsigned long flags;
106
107	spin_lock_irqsave(&adapter->ptp_lock, flags);
108
109	tsnep_get_system_time(adapter, &system_time);
110
111	system_time += delta;
112
113	/* high dword is buffered in hardware and synchronously written to
114	 * system time when low dword is written
115	 */
116	iowrite32(system_time >> 32, adapter->addr + ECM_SYSTEM_TIME_HIGH);
117	iowrite32(system_time & 0xFFFFFFFF,
118		  adapter->addr + ECM_SYSTEM_TIME_LOW);
119
120	spin_unlock_irqrestore(&adapter->ptp_lock, flags);
121
122	return 0;
123}
124
125static int tsnep_ptp_gettimex64(struct ptp_clock_info *ptp,
126				struct timespec64 *ts,
127				struct ptp_system_timestamp *sts)
128{
129	struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
130						     ptp_clock_info);
131	u32 high_before;
132	u32 low;
133	u32 high;
134	u64 system_time;
135
136	/* read high dword twice to detect overrun */
137	high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH);
138	do {
139		ptp_read_system_prets(sts);
140		low = ioread32(adapter->addr + ECM_SYSTEM_TIME_LOW);
141		ptp_read_system_postts(sts);
142		high_before = high;
143		high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH);
144	} while (high != high_before);
145	system_time = (((u64)high) << 32) | ((u64)low);
146
147	*ts = ns_to_timespec64(system_time);
148
149	return 0;
150}
151
152static int tsnep_ptp_settime64(struct ptp_clock_info *ptp,
153			       const struct timespec64 *ts)
154{
155	struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
156						     ptp_clock_info);
157	u64 system_time = timespec64_to_ns(ts);
158	unsigned long flags;
159
160	spin_lock_irqsave(&adapter->ptp_lock, flags);
161
162	/* high dword is buffered in hardware and synchronously written to
163	 * system time when low dword is written
164	 */
165	iowrite32(system_time >> 32, adapter->addr + ECM_SYSTEM_TIME_HIGH);
166	iowrite32(system_time & 0xFFFFFFFF,
167		  adapter->addr + ECM_SYSTEM_TIME_LOW);
168
169	spin_unlock_irqrestore(&adapter->ptp_lock, flags);
170
171	return 0;
172}
173
174static int tsnep_ptp_getcyclesx64(struct ptp_clock_info *ptp,
175				  struct timespec64 *ts,
176				  struct ptp_system_timestamp *sts)
177{
178	struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
179						     ptp_clock_info);
180	u32 high_before;
181	u32 low;
182	u32 high;
183	u64 counter;
184
185	/* read high dword twice to detect overrun */
186	high = ioread32(adapter->addr + ECM_COUNTER_HIGH);
187	do {
188		ptp_read_system_prets(sts);
189		low = ioread32(adapter->addr + ECM_COUNTER_LOW);
190		ptp_read_system_postts(sts);
191		high_before = high;
192		high = ioread32(adapter->addr + ECM_COUNTER_HIGH);
193	} while (high != high_before);
194	counter = (((u64)high) << 32) | ((u64)low);
195
196	*ts = ns_to_timespec64(counter);
197
198	return 0;
199}
200
201int tsnep_ptp_init(struct tsnep_adapter *adapter)
202{
203	int retval = 0;
204
205	adapter->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_NONE;
206	adapter->hwtstamp_config.tx_type = HWTSTAMP_TX_OFF;
207
208	snprintf(adapter->ptp_clock_info.name, 16, "%s", TSNEP);
209	adapter->ptp_clock_info.owner = THIS_MODULE;
210	/* at most 2^-1ns adjustment every clock cycle for 8ns clock cycle time,
211	 * stay slightly below because only bits below 2^-1ns are supported
212	 */
213	adapter->ptp_clock_info.max_adj = (500000000 / 8 - 1);
214	adapter->ptp_clock_info.adjfine = tsnep_ptp_adjfine;
215	adapter->ptp_clock_info.adjtime = tsnep_ptp_adjtime;
216	adapter->ptp_clock_info.gettimex64 = tsnep_ptp_gettimex64;
217	adapter->ptp_clock_info.settime64 = tsnep_ptp_settime64;
218	adapter->ptp_clock_info.getcyclesx64 = tsnep_ptp_getcyclesx64;
219
220	spin_lock_init(&adapter->ptp_lock);
221
222	adapter->ptp_clock = ptp_clock_register(&adapter->ptp_clock_info,
223						&adapter->pdev->dev);
224	if (IS_ERR(adapter->ptp_clock)) {
225		netdev_err(adapter->netdev, "ptp_clock_register failed\n");
226
227		retval = PTR_ERR(adapter->ptp_clock);
228		adapter->ptp_clock = NULL;
229	} else if (adapter->ptp_clock) {
230		netdev_info(adapter->netdev, "PHC added\n");
231	}
232
233	return retval;
234}
235
236void tsnep_ptp_cleanup(struct tsnep_adapter *adapter)
237{
238	if (adapter->ptp_clock) {
239		ptp_clock_unregister(adapter->ptp_clock);
240		netdev_info(adapter->netdev, "PHC removed\n");
241	}
242}