tsnep_ptp.c (6807B)
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}