stmicro/stmmac/stmmac_ptp.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*******************************************************************************
0003   PTP 1588 clock using the STMMAC.
0004
0005   Copyright (C) 2013  Vayavya Labs Pvt Ltd
0006
0007
0008   Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
0009 *******************************************************************************/
0010 #include "stmmac.h"
0011 #include "stmmac_ptp.h"
0012 #include "dwmac4.h"
0013
0014 /**
0015  * stmmac_adjust_freq
0016  *
0017  * @ptp: pointer to ptp_clock_info structure
0018  * @ppb: desired period change in parts ber billion
0019  *
0020  * Description: this function will adjust the frequency of hardware clock.
0021  */
0022 static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
0023 {
0024     struct stmmac_priv *priv =
0025         container_of(ptp, struct stmmac_priv, ptp_clock_ops);
0026     unsigned long flags;
0027     u32 diff, addend;
0028     int neg_adj = 0;
0029     u64 adj;
0030
0031     if (ppb < 0) {
0032         neg_adj = 1;
0033         ppb = -ppb;
0034     }
0035
0036     addend = priv->default_addend;
0037     adj = addend;
0038     adj *= ppb;
0039     diff = div_u64(adj, 1000000000ULL);
0040     addend = neg_adj ? (addend - diff) : (addend + diff);
0041
0042     write_lock_irqsave(&priv->ptp_lock, flags);
0043     stmmac_config_addend(priv, priv->ptpaddr, addend);
0044     write_unlock_irqrestore(&priv->ptp_lock, flags);
0045
0046     return 0;
0047 }
0048
0049 /**
0050  * stmmac_adjust_time
0051  *
0052  * @ptp: pointer to ptp_clock_info structure
0053  * @delta: desired change in nanoseconds
0054  *
0055  * Description: this function will shift/adjust the hardware clock time.
0056  */
0057 static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
0058 {
0059     struct stmmac_priv *priv =
0060         container_of(ptp, struct stmmac_priv, ptp_clock_ops);
0061     unsigned long flags;
0062     u32 sec, nsec;
0063     u32 quotient, reminder;
0064     int neg_adj = 0;
0065     bool xmac, est_rst = false;
0066     int ret;
0067
0068     xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;
0069
0070     if (delta < 0) {
0071         neg_adj = 1;
0072         delta = -delta;
0073     }
0074
0075     quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
0076     sec = quotient;
0077     nsec = reminder;
0078
0079     /* If EST is enabled, disabled it before adjust ptp time. */
0080     if (priv->plat->est && priv->plat->est->enable) {
0081         est_rst = true;
0082         mutex_lock(&priv->plat->est->lock);
0083         priv->plat->est->enable = false;
0084         stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
0085                      priv->plat->clk_ptp_rate);
0086         mutex_unlock(&priv->plat->est->lock);
0087     }
0088
0089     write_lock_irqsave(&priv->ptp_lock, flags);
0090     stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
0091     write_unlock_irqrestore(&priv->ptp_lock, flags);
0092
0093     /* Caculate new basetime and re-configured EST after PTP time adjust. */
0094     if (est_rst) {
0095         struct timespec64 current_time, time;
0096         ktime_t current_time_ns, basetime;
0097         u64 cycle_time;
0098
0099         mutex_lock(&priv->plat->est->lock);
0100         priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, &current_time);
0101         current_time_ns = timespec64_to_ktime(current_time);
0102         time.tv_nsec = priv->plat->est->btr_reserve[0];
0103         time.tv_sec = priv->plat->est->btr_reserve[1];
0104         basetime = timespec64_to_ktime(time);
0105         cycle_time = (u64)priv->plat->est->ctr[1] * NSEC_PER_SEC +
0106                  priv->plat->est->ctr[0];
0107         time = stmmac_calc_tas_basetime(basetime,
0108                         current_time_ns,
0109                         cycle_time);
0110
0111         priv->plat->est->btr[0] = (u32)time.tv_nsec;
0112         priv->plat->est->btr[1] = (u32)time.tv_sec;
0113         priv->plat->est->enable = true;
0114         ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
0115                        priv->plat->clk_ptp_rate);
0116         mutex_unlock(&priv->plat->est->lock);
0117         if (ret)
0118             netdev_err(priv->dev, "failed to configure EST\n");
0119     }
0120
0121     return 0;
0122 }
0123
0124 /**
0125  * stmmac_get_time
0126  *
0127  * @ptp: pointer to ptp_clock_info structure
0128  * @ts: pointer to hold time/result
0129  *
0130  * Description: this function will read the current time from the
0131  * hardware clock and store it in @ts.
0132  */
0133 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
0134 {
0135     struct stmmac_priv *priv =
0136         container_of(ptp, struct stmmac_priv, ptp_clock_ops);
0137     unsigned long flags;
0138     u64 ns = 0;
0139
0140     read_lock_irqsave(&priv->ptp_lock, flags);
0141     stmmac_get_systime(priv, priv->ptpaddr, &ns);
0142     read_unlock_irqrestore(&priv->ptp_lock, flags);
0143
0144     *ts = ns_to_timespec64(ns);
0145
0146     return 0;
0147 }
0148
0149 /**
0150  * stmmac_set_time
0151  *
0152  * @ptp: pointer to ptp_clock_info structure
0153  * @ts: time value to set
0154  *
0155  * Description: this function will set the current time on the
0156  * hardware clock.
0157  */
0158 static int stmmac_set_time(struct ptp_clock_info *ptp,
0159                const struct timespec64 *ts)
0160 {
0161     struct stmmac_priv *priv =
0162         container_of(ptp, struct stmmac_priv, ptp_clock_ops);
0163     unsigned long flags;
0164
0165     write_lock_irqsave(&priv->ptp_lock, flags);
0166     stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
0167     write_unlock_irqrestore(&priv->ptp_lock, flags);
0168
0169     return 0;
0170 }
0171
0172 static int stmmac_enable(struct ptp_clock_info *ptp,
0173              struct ptp_clock_request *rq, int on)
0174 {
0175     struct stmmac_priv *priv =
0176         container_of(ptp, struct stmmac_priv, ptp_clock_ops);
0177     void __iomem *ptpaddr = priv->ptpaddr;
0178     struct stmmac_pps_cfg *cfg;
0179     int ret = -EOPNOTSUPP;
0180     unsigned long flags;
0181     u32 acr_value;
0182
0183     switch (rq->type) {
0184     case PTP_CLK_REQ_PEROUT:
0185         /* Reject requests with unsupported flags */
0186         if (rq->perout.flags)
0187             return -EOPNOTSUPP;
0188
0189         cfg = &priv->pps[rq->perout.index];
0190
0191         cfg->start.tv_sec = rq->perout.start.sec;
0192         cfg->start.tv_nsec = rq->perout.start.nsec;
0193         cfg->period.tv_sec = rq->perout.period.sec;
0194         cfg->period.tv_nsec = rq->perout.period.nsec;
0195
0196         write_lock_irqsave(&priv->ptp_lock, flags);
0197         ret = stmmac_flex_pps_config(priv, priv->ioaddr,
0198                          rq->perout.index, cfg, on,
0199                          priv->sub_second_inc,
0200                          priv->systime_flags);
0201         write_unlock_irqrestore(&priv->ptp_lock, flags);
0202         break;
0203     case PTP_CLK_REQ_EXTTS:
0204         priv->plat->ext_snapshot_en = on;
0205         mutex_lock(&priv->aux_ts_lock);
0206         acr_value = readl(ptpaddr + PTP_ACR);
0207         acr_value &= ~PTP_ACR_MASK;
0208         if (on) {
0209             /* Enable External snapshot trigger */
0210             acr_value |= priv->plat->ext_snapshot_num;
0211             acr_value |= PTP_ACR_ATSFC;
0212             netdev_dbg(priv->dev, "Auxiliary Snapshot %d enabled.\n",
0213                    priv->plat->ext_snapshot_num >>
0214                    PTP_ACR_ATSEN_SHIFT);
0215         } else {
0216             netdev_dbg(priv->dev, "Auxiliary Snapshot %d disabled.\n",
0217                    priv->plat->ext_snapshot_num >>
0218                    PTP_ACR_ATSEN_SHIFT);
0219         }
0220         writel(acr_value, ptpaddr + PTP_ACR);
0221         mutex_unlock(&priv->aux_ts_lock);
0222         ret = 0;
0223         break;
0224
0225     default:
0226         break;
0227     }
0228
0229     return ret;
0230 }
0231
0232 /**
0233  * stmmac_get_syncdevicetime
0234  * @device: current device time
0235  * @system: system counter value read synchronously with device time
0236  * @ctx: context provided by timekeeping code
0237  * Description: Read device and system clock simultaneously and return the
0238  * corrected clock values in ns.
0239  **/
0240 static int stmmac_get_syncdevicetime(ktime_t *device,
0241                      struct system_counterval_t *system,
0242                      void *ctx)
0243 {
0244     struct stmmac_priv *priv = (struct stmmac_priv *)ctx;
0245
0246     if (priv->plat->crosststamp)
0247         return priv->plat->crosststamp(device, system, ctx);
0248     else
0249         return -EOPNOTSUPP;
0250 }
0251
0252 static int stmmac_getcrosststamp(struct ptp_clock_info *ptp,
0253                  struct system_device_crosststamp *xtstamp)
0254 {
0255     struct stmmac_priv *priv =
0256         container_of(ptp, struct stmmac_priv, ptp_clock_ops);
0257
0258     return get_device_system_crosststamp(stmmac_get_syncdevicetime,
0259                          priv, NULL, xtstamp);
0260 }
0261
0262 /* structure describing a PTP hardware clock */
0263 static struct ptp_clock_info stmmac_ptp_clock_ops = {
0264     .owner = THIS_MODULE,
0265     .name = "stmmac ptp",
0266     .max_adj = 62500000,
0267     .n_alarm = 0,
0268     .n_ext_ts = 0, /* will be overwritten in stmmac_ptp_register */
0269     .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */
0270     .n_pins = 0,
0271     .pps = 0,
0272     .adjfreq = stmmac_adjust_freq,
0273     .adjtime = stmmac_adjust_time,
0274     .gettime64 = stmmac_get_time,
0275     .settime64 = stmmac_set_time,
0276     .enable = stmmac_enable,
0277     .getcrosststamp = stmmac_getcrosststamp,
0278 };
0279
0280 /**
0281  * stmmac_ptp_register
0282  * @priv: driver private structure
0283  * Description: this function will register the ptp clock driver
0284  * to kernel. It also does some house keeping work.
0285  */
0286 void stmmac_ptp_register(struct stmmac_priv *priv)
0287 {
0288     int i;
0289
0290     for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
0291         if (i >= STMMAC_PPS_MAX)
0292             break;
0293         priv->pps[i].available = true;
0294     }
0295
0296     if (priv->plat->ptp_max_adj)
0297         stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;
0298
0299     /* Calculate the clock domain crossing (CDC) error if necessary */
0300     priv->plat->cdc_error_adj = 0;
0301     if (priv->plat->has_gmac4 && priv->plat->clk_ptp_rate)
0302         priv->plat->cdc_error_adj = (2 * NSEC_PER_SEC) / priv->plat->clk_ptp_rate;
0303
0304     stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
0305     stmmac_ptp_clock_ops.n_ext_ts = priv->dma_cap.aux_snapshot_n;
0306
0307     rwlock_init(&priv->ptp_lock);
0308     mutex_init(&priv->aux_ts_lock);
0309     priv->ptp_clock_ops = stmmac_ptp_clock_ops;
0310
0311     priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
0312                          priv->device);
0313     if (IS_ERR(priv->ptp_clock)) {
0314         netdev_err(priv->dev, "ptp_clock_register failed\n");
0315         priv->ptp_clock = NULL;
0316     } else if (priv->ptp_clock)
0317         netdev_info(priv->dev, "registered PTP clock\n");
0318 }
0319
0320 /**
0321  * stmmac_ptp_unregister
0322  * @priv: driver private structure
0323  * Description: this function will remove/unregister the ptp clock driver
0324  * from the kernel.
0325  */
0326 void stmmac_ptp_unregister(struct stmmac_priv *priv)
0327 {
0328     if (priv->ptp_clock) {
0329         ptp_clock_unregister(priv->ptp_clock);
0330         priv->ptp_clock = NULL;
0331         pr_debug("Removed PTP HW clock successfully on %s\n",
0332              priv->dev->name);
0333     }
0334
0335     mutex_destroy(&priv->aux_ts_lock);
0336 }