ethernet/engleder/tsnep_ptp.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
0003
0004 #include "tsnep.h"
0005
0006 void tsnep_get_system_time(struct tsnep_adapter *adapter, u64 *time)
0007 {
0008     u32 high_before;
0009     u32 low;
0010     u32 high;
0011
0012     /* read high dword twice to detect overrun */
0013     high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH);
0014     do {
0015         low = ioread32(adapter->addr + ECM_SYSTEM_TIME_LOW);
0016         high_before = high;
0017         high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH);
0018     } while (high != high_before);
0019     *time = (((u64)high) << 32) | ((u64)low);
0020 }
0021
0022 int tsnep_ptp_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
0023 {
0024     struct tsnep_adapter *adapter = netdev_priv(netdev);
0025     struct hwtstamp_config config;
0026
0027     if (!ifr)
0028         return -EINVAL;
0029
0030     if (cmd == SIOCSHWTSTAMP) {
0031         if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
0032             return -EFAULT;
0033
0034         switch (config.tx_type) {
0035         case HWTSTAMP_TX_OFF:
0036         case HWTSTAMP_TX_ON:
0037             break;
0038         default:
0039             return -ERANGE;
0040         }
0041
0042         switch (config.rx_filter) {
0043         case HWTSTAMP_FILTER_NONE:
0044             break;
0045         case HWTSTAMP_FILTER_ALL:
0046         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
0047         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
0048         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
0049         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
0050         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
0051         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
0052         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
0053         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
0054         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
0055         case HWTSTAMP_FILTER_PTP_V2_EVENT:
0056         case HWTSTAMP_FILTER_PTP_V2_SYNC:
0057         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
0058         case HWTSTAMP_FILTER_NTP_ALL:
0059             config.rx_filter = HWTSTAMP_FILTER_ALL;
0060             break;
0061         default:
0062             return -ERANGE;
0063         }
0064
0065         memcpy(&adapter->hwtstamp_config, &config,
0066                sizeof(adapter->hwtstamp_config));
0067     }
0068
0069     if (copy_to_user(ifr->ifr_data, &adapter->hwtstamp_config,
0070              sizeof(adapter->hwtstamp_config)))
0071         return -EFAULT;
0072
0073     return 0;
0074 }
0075
0076 static int tsnep_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
0077 {
0078     struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
0079                              ptp_clock_info);
0080     bool negative = false;
0081     u64 rate_offset;
0082
0083     if (scaled_ppm < 0) {
0084         scaled_ppm = -scaled_ppm;
0085         negative = true;
0086     }
0087
0088     /* convert from 16 bit to 32 bit binary fractional, divide by 1000000 to
0089      * eliminate ppm, multiply with 8 to compensate 8ns clock cycle time,
0090      * simplify calculation because 15625 * 8 = 1000000 / 8
0091      */
0092     rate_offset = scaled_ppm;
0093     rate_offset <<= 16 - 3;
0094     rate_offset = div_u64(rate_offset, 15625);
0095
0096     rate_offset &= ECM_CLOCK_RATE_OFFSET_MASK;
0097     if (negative)
0098         rate_offset |= ECM_CLOCK_RATE_OFFSET_SIGN;
0099     iowrite32(rate_offset & 0xFFFFFFFF, adapter->addr + ECM_CLOCK_RATE);
0100
0101     return 0;
0102 }
0103
0104 static int tsnep_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
0105 {
0106     struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
0107                              ptp_clock_info);
0108     u64 system_time;
0109     unsigned long flags;
0110
0111     spin_lock_irqsave(&adapter->ptp_lock, flags);
0112
0113     tsnep_get_system_time(adapter, &system_time);
0114
0115     system_time += delta;
0116
0117     /* high dword is buffered in hardware and synchronously written to
0118      * system time when low dword is written
0119      */
0120     iowrite32(system_time >> 32, adapter->addr + ECM_SYSTEM_TIME_HIGH);
0121     iowrite32(system_time & 0xFFFFFFFF,
0122           adapter->addr + ECM_SYSTEM_TIME_LOW);
0123
0124     spin_unlock_irqrestore(&adapter->ptp_lock, flags);
0125
0126     return 0;
0127 }
0128
0129 static int tsnep_ptp_gettimex64(struct ptp_clock_info *ptp,
0130                 struct timespec64 *ts,
0131                 struct ptp_system_timestamp *sts)
0132 {
0133     struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
0134                              ptp_clock_info);
0135     u32 high_before;
0136     u32 low;
0137     u32 high;
0138     u64 system_time;
0139
0140     /* read high dword twice to detect overrun */
0141     high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH);
0142     do {
0143         ptp_read_system_prets(sts);
0144         low = ioread32(adapter->addr + ECM_SYSTEM_TIME_LOW);
0145         ptp_read_system_postts(sts);
0146         high_before = high;
0147         high = ioread32(adapter->addr + ECM_SYSTEM_TIME_HIGH);
0148     } while (high != high_before);
0149     system_time = (((u64)high) << 32) | ((u64)low);
0150
0151     *ts = ns_to_timespec64(system_time);
0152
0153     return 0;
0154 }
0155
0156 static int tsnep_ptp_settime64(struct ptp_clock_info *ptp,
0157                    const struct timespec64 *ts)
0158 {
0159     struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
0160                              ptp_clock_info);
0161     u64 system_time = timespec64_to_ns(ts);
0162     unsigned long flags;
0163
0164     spin_lock_irqsave(&adapter->ptp_lock, flags);
0165
0166     /* high dword is buffered in hardware and synchronously written to
0167      * system time when low dword is written
0168      */
0169     iowrite32(system_time >> 32, adapter->addr + ECM_SYSTEM_TIME_HIGH);
0170     iowrite32(system_time & 0xFFFFFFFF,
0171           adapter->addr + ECM_SYSTEM_TIME_LOW);
0172
0173     spin_unlock_irqrestore(&adapter->ptp_lock, flags);
0174
0175     return 0;
0176 }
0177
0178 static int tsnep_ptp_getcyclesx64(struct ptp_clock_info *ptp,
0179                   struct timespec64 *ts,
0180                   struct ptp_system_timestamp *sts)
0181 {
0182     struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter,
0183                              ptp_clock_info);
0184     u32 high_before;
0185     u32 low;
0186     u32 high;
0187     u64 counter;
0188
0189     /* read high dword twice to detect overrun */
0190     high = ioread32(adapter->addr + ECM_COUNTER_HIGH);
0191     do {
0192         ptp_read_system_prets(sts);
0193         low = ioread32(adapter->addr + ECM_COUNTER_LOW);
0194         ptp_read_system_postts(sts);
0195         high_before = high;
0196         high = ioread32(adapter->addr + ECM_COUNTER_HIGH);
0197     } while (high != high_before);
0198     counter = (((u64)high) << 32) | ((u64)low);
0199
0200     *ts = ns_to_timespec64(counter);
0201
0202     return 0;
0203 }
0204
0205 int tsnep_ptp_init(struct tsnep_adapter *adapter)
0206 {
0207     int retval = 0;
0208
0209     adapter->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_NONE;
0210     adapter->hwtstamp_config.tx_type = HWTSTAMP_TX_OFF;
0211
0212     snprintf(adapter->ptp_clock_info.name, 16, "%s", TSNEP);
0213     adapter->ptp_clock_info.owner = THIS_MODULE;
0214     /* at most 2^-1ns adjustment every clock cycle for 8ns clock cycle time,
0215      * stay slightly below because only bits below 2^-1ns are supported
0216      */
0217     adapter->ptp_clock_info.max_adj = (500000000 / 8 - 1);
0218     adapter->ptp_clock_info.adjfine = tsnep_ptp_adjfine;
0219     adapter->ptp_clock_info.adjtime = tsnep_ptp_adjtime;
0220     adapter->ptp_clock_info.gettimex64 = tsnep_ptp_gettimex64;
0221     adapter->ptp_clock_info.settime64 = tsnep_ptp_settime64;
0222     adapter->ptp_clock_info.getcyclesx64 = tsnep_ptp_getcyclesx64;
0223
0224     spin_lock_init(&adapter->ptp_lock);
0225
0226     adapter->ptp_clock = ptp_clock_register(&adapter->ptp_clock_info,
0227                         &adapter->pdev->dev);
0228     if (IS_ERR(adapter->ptp_clock)) {
0229         netdev_err(adapter->netdev, "ptp_clock_register failed\n");
0230
0231         retval = PTR_ERR(adapter->ptp_clock);
0232         adapter->ptp_clock = NULL;
0233     } else if (adapter->ptp_clock) {
0234         netdev_info(adapter->netdev, "PHC added\n");
0235     }
0236
0237     return retval;
0238 }
0239
0240 void tsnep_ptp_cleanup(struct tsnep_adapter *adapter)
0241 {
0242     if (adapter->ptp_clock) {
0243         ptp_clock_unregister(adapter->ptp_clock);
0244         netdev_info(adapter->netdev, "PHC removed\n");
0245     }
0246 }