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 if (config.flags) 35 return -EINVAL; 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 memcpy(&adapter->hwtstamp_config, &config, 69 sizeof(adapter->hwtstamp_config)); 70 } 71 72 if (copy_to_user(ifr->ifr_data, &adapter->hwtstamp_config, 73 sizeof(adapter->hwtstamp_config))) 74 return -EFAULT; 75 76 return 0; 77 } 78 79 static int tsnep_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 80 { 81 struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, 82 ptp_clock_info); 83 bool negative = false; 84 u64 rate_offset; 85 86 if (scaled_ppm < 0) { 87 scaled_ppm = -scaled_ppm; 88 negative = true; 89 } 90 91 /* convert from 16 bit to 32 bit binary fractional, divide by 1000000 to 92 * eliminate ppm, multiply with 8 to compensate 8ns clock cycle time, 93 * simplify calculation because 15625 * 8 = 1000000 / 8 94 */ 95 rate_offset = scaled_ppm; 96 rate_offset <<= 16 - 3; 97 rate_offset = div_u64(rate_offset, 15625); 98 99 rate_offset &= ECM_CLOCK_RATE_OFFSET_MASK; 100 if (negative) 101 rate_offset |= ECM_CLOCK_RATE_OFFSET_SIGN; 102 iowrite32(rate_offset & 0xFFFFFFFF, adapter->addr + ECM_CLOCK_RATE); 103 104 return 0; 105 } 106 107 static int tsnep_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 108 { 109 struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, 110 ptp_clock_info); 111 u64 system_time; 112 unsigned long flags; 113 114 spin_lock_irqsave(&adapter->ptp_lock, flags); 115 116 tsnep_get_system_time(adapter, &system_time); 117 118 system_time += delta; 119 120 /* high dword is buffered in hardware and synchronously written to 121 * system time when low dword is written 122 */ 123 iowrite32(system_time >> 32, adapter->addr + ECM_SYSTEM_TIME_HIGH); 124 iowrite32(system_time & 0xFFFFFFFF, 125 adapter->addr + ECM_SYSTEM_TIME_LOW); 126 127 spin_unlock_irqrestore(&adapter->ptp_lock, flags); 128 129 return 0; 130 } 131 132 static int tsnep_ptp_gettimex64(struct ptp_clock_info *ptp, 133 struct timespec64 *ts, 134 struct ptp_system_timestamp *sts) 135 { 136 struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, 137 ptp_clock_info); 138 u32 high_before; 139 u32 low; 140 u32 high; 141 u64 system_time; 142 143 /* read high dword twice to detect overrun */ 144 high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH); 145 do { 146 ptp_read_system_prets(sts); 147 low = ioread32(adapter->addr + ECM_SYSTEM_TIME_LOW); 148 ptp_read_system_postts(sts); 149 high_before = high; 150 high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH); 151 } while (high != high_before); 152 system_time = (((u64)high) << 32) | ((u64)low); 153 154 *ts = ns_to_timespec64(system_time); 155 156 return 0; 157 } 158 159 static int tsnep_ptp_settime64(struct ptp_clock_info *ptp, 160 const struct timespec64 *ts) 161 { 162 struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, 163 ptp_clock_info); 164 u64 system_time = timespec64_to_ns(ts); 165 unsigned long flags; 166 167 spin_lock_irqsave(&adapter->ptp_lock, flags); 168 169 /* high dword is buffered in hardware and synchronously written to 170 * system time when low dword is written 171 */ 172 iowrite32(system_time >> 32, adapter->addr + ECM_SYSTEM_TIME_HIGH); 173 iowrite32(system_time & 0xFFFFFFFF, 174 adapter->addr + ECM_SYSTEM_TIME_LOW); 175 176 spin_unlock_irqrestore(&adapter->ptp_lock, flags); 177 178 return 0; 179 } 180 181 int tsnep_ptp_init(struct tsnep_adapter *adapter) 182 { 183 int retval = 0; 184 185 adapter->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_NONE; 186 adapter->hwtstamp_config.tx_type = HWTSTAMP_TX_OFF; 187 188 snprintf(adapter->ptp_clock_info.name, 16, "%s", TSNEP); 189 adapter->ptp_clock_info.owner = THIS_MODULE; 190 /* at most 2^-1ns adjustment every clock cycle for 8ns clock cycle time, 191 * stay slightly below because only bits below 2^-1ns are supported 192 */ 193 adapter->ptp_clock_info.max_adj = (500000000 / 8 - 1); 194 adapter->ptp_clock_info.adjfine = tsnep_ptp_adjfine; 195 adapter->ptp_clock_info.adjtime = tsnep_ptp_adjtime; 196 adapter->ptp_clock_info.gettimex64 = tsnep_ptp_gettimex64; 197 adapter->ptp_clock_info.settime64 = tsnep_ptp_settime64; 198 199 spin_lock_init(&adapter->ptp_lock); 200 201 adapter->ptp_clock = ptp_clock_register(&adapter->ptp_clock_info, 202 &adapter->pdev->dev); 203 if (IS_ERR(adapter->ptp_clock)) { 204 netdev_err(adapter->netdev, "ptp_clock_register failed\n"); 205 206 retval = PTR_ERR(adapter->ptp_clock); 207 adapter->ptp_clock = NULL; 208 } else if (adapter->ptp_clock) { 209 netdev_info(adapter->netdev, "PHC added\n"); 210 } 211 212 return retval; 213 } 214 215 void tsnep_ptp_cleanup(struct tsnep_adapter *adapter) 216 { 217 if (adapter->ptp_clock) { 218 ptp_clock_unregister(adapter->ptp_clock); 219 netdev_info(adapter->netdev, "PHC removed\n"); 220 } 221 } 222