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_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 31 int 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 72 static 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 100 static 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 125 static 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 152 static 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 174 static 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 201 int 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 236 void 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 } 243