247 lines · c
1// SPDX-License-Identifier: GPL-2.02/* 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 to89 * eliminate ppm, multiply with 8 to compensate 8ns clock cycle time,90 * simplify calculation because 15625 * 8 = 1000000 / 891 */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 to118 * system time when low dword is written119 */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 to167 * system time when low dword is written168 */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 supported216 */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}247