drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*******************************************************************************
   3   PTP 1588 clock using the STMMAC.
   4
   5   Copyright (C) 2013  Vayavya Labs Pvt Ltd
   6
   7
   8   Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
   9 *******************************************************************************/
  10 #include "stmmac.h"
  11 #include "stmmac_ptp.h"
  12 #include "dwmac4.h"
  13
  14 /**
  15  * stmmac_adjust_freq
  16  *
  17  * @ptp: pointer to ptp_clock_info structure
  18  * @ppb: desired period change in parts ber billion
  19  *
  20  * Description: this function will adjust the frequency of hardware clock.
  21  */
  22 static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
  23 {
  24         struct stmmac_priv *priv =
  25             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
  26         unsigned long flags;
  27         u32 diff, addend;
  28         int neg_adj = 0;
  29         u64 adj;
  30
  31         if (ppb < 0) {
  32                 neg_adj = 1;
  33                 ppb = -ppb;
  34         }
  35
  36         addend = priv->default_addend;
  37         adj = addend;
  38         adj *= ppb;
  39         diff = div_u64(adj, 1000000000ULL);
  40         addend = neg_adj ? (addend - diff) : (addend + diff);
  41
  42         spin_lock_irqsave(&priv->ptp_lock, flags);
  43         stmmac_config_addend(priv, priv->ptpaddr, addend);
  44         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  45
  46         return 0;
  47 }
  48
  49 /**
  50  * stmmac_adjust_time
  51  *
  52  * @ptp: pointer to ptp_clock_info structure
  53  * @delta: desired change in nanoseconds
  54  *
  55  * Description: this function will shift/adjust the hardware clock time.
  56  */
  57 static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
  58 {
  59         struct stmmac_priv *priv =
  60             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
  61         unsigned long flags;
  62         u32 sec, nsec;
  63         u32 quotient, reminder;
  64         int neg_adj = 0;
  65         bool xmac;
  66
  67         xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;
  68
  69         if (delta < 0) {
  70                 neg_adj = 1;
  71                 delta = -delta;
  72         }
  73
  74         quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
  75         sec = quotient;
  76         nsec = reminder;
  77
  78         spin_lock_irqsave(&priv->ptp_lock, flags);
  79         stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
  80         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  81
  82         return 0;
  83 }
  84
  85 /**
  86  * stmmac_get_time
  87  *
  88  * @ptp: pointer to ptp_clock_info structure
  89  * @ts: pointer to hold time/result
  90  *
  91  * Description: this function will read the current time from the
  92  * hardware clock and store it in @ts.
  93  */
  94 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
  95 {
  96         struct stmmac_priv *priv =
  97             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
  98         unsigned long flags;
  99         u64 ns = 0;
 100
 101         spin_lock_irqsave(&priv->ptp_lock, flags);
 102         stmmac_get_systime(priv, priv->ptpaddr, &ns);
 103         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 104
 105         *ts = ns_to_timespec64(ns);
 106
 107         return 0;
 108 }
 109
 110 /**
 111  * stmmac_set_time
 112  *
 113  * @ptp: pointer to ptp_clock_info structure
 114  * @ts: time value to set
 115  *
 116  * Description: this function will set the current time on the
 117  * hardware clock.
 118  */
 119 static int stmmac_set_time(struct ptp_clock_info *ptp,
 120                            const struct timespec64 *ts)
 121 {
 122         struct stmmac_priv *priv =
 123             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 124         unsigned long flags;
 125
 126         spin_lock_irqsave(&priv->ptp_lock, flags);
 127         stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
 128         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 129
 130         return 0;
 131 }
 132
 133 static int stmmac_enable(struct ptp_clock_info *ptp,
 134                          struct ptp_clock_request *rq, int on)
 135 {
 136         struct stmmac_priv *priv =
 137             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 138         void __iomem *ptpaddr = priv->ptpaddr;
 139         void __iomem *ioaddr = priv->hw->pcsr;
 140         struct stmmac_pps_cfg *cfg;
 141         u32 intr_value, acr_value;
 142         int ret = -EOPNOTSUPP;
 143         unsigned long flags;
 144
 145         switch (rq->type) {
 146         case PTP_CLK_REQ_PEROUT:
 147                 /* Reject requests with unsupported flags */
 148                 if (rq->perout.flags)
 149                         return -EOPNOTSUPP;
 150
 151                 cfg = &priv->pps[rq->perout.index];
 152
 153                 cfg->start.tv_sec = rq->perout.start.sec;
 154                 cfg->start.tv_nsec = rq->perout.start.nsec;
 155                 cfg->period.tv_sec = rq->perout.period.sec;
 156                 cfg->period.tv_nsec = rq->perout.period.nsec;
 157
 158                 spin_lock_irqsave(&priv->ptp_lock, flags);
 159                 ret = stmmac_flex_pps_config(priv, priv->ioaddr,
 160                                              rq->perout.index, cfg, on,
 161                                              priv->sub_second_inc,
 162                                              priv->systime_flags);
 163                 spin_unlock_irqrestore(&priv->ptp_lock, flags);
 164                 break;
 165         case PTP_CLK_REQ_EXTTS:
 166                 priv->plat->ext_snapshot_en = on;
 167                 mutex_lock(&priv->aux_ts_lock);
 168                 acr_value = readl(ptpaddr + PTP_ACR);
 169                 acr_value &= ~PTP_ACR_MASK;
 170                 if (on) {
 171                         /* Enable External snapshot trigger */
 172                         acr_value |= priv->plat->ext_snapshot_num;
 173                         acr_value |= PTP_ACR_ATSFC;
 174                         netdev_dbg(priv->dev, "Auxiliary Snapshot %d enabled.\n",
 175                                    priv->plat->ext_snapshot_num >>
 176                                    PTP_ACR_ATSEN_SHIFT);
 177                         /* Enable Timestamp Interrupt */
 178                         intr_value = readl(ioaddr + GMAC_INT_EN);
 179                         intr_value |= GMAC_INT_TSIE;
 180                         writel(intr_value, ioaddr + GMAC_INT_EN);
 181
 182                 } else {
 183                         netdev_dbg(priv->dev, "Auxiliary Snapshot %d disabled.\n",
 184                                    priv->plat->ext_snapshot_num >>
 185                                    PTP_ACR_ATSEN_SHIFT);
 186                         /* Disable Timestamp Interrupt */
 187                         intr_value = readl(ioaddr + GMAC_INT_EN);
 188                         intr_value &= ~GMAC_INT_TSIE;
 189                         writel(intr_value, ioaddr + GMAC_INT_EN);
 190                 }
 191                 writel(acr_value, ptpaddr + PTP_ACR);
 192                 mutex_unlock(&priv->aux_ts_lock);
 193                 ret = 0;
 194                 break;
 195
 196         default:
 197                 break;
 198         }
 199
 200         return ret;
 201 }
 202
 203 /**
 204  * stmmac_get_syncdevicetime
 205  * @device: current device time
 206  * @system: system counter value read synchronously with device time
 207  * @ctx: context provided by timekeeping code
 208  * Description: Read device and system clock simultaneously and return the
 209  * corrected clock values in ns.
 210  **/
 211 static int stmmac_get_syncdevicetime(ktime_t *device,
 212                                      struct system_counterval_t *system,
 213                                      void *ctx)
 214 {
 215         struct stmmac_priv *priv = (struct stmmac_priv *)ctx;
 216
 217         if (priv->plat->crosststamp)
 218                 return priv->plat->crosststamp(device, system, ctx);
 219         else
 220                 return -EOPNOTSUPP;
 221 }
 222
 223 static int stmmac_getcrosststamp(struct ptp_clock_info *ptp,
 224                                  struct system_device_crosststamp *xtstamp)
 225 {
 226         struct stmmac_priv *priv =
 227                 container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 228
 229         return get_device_system_crosststamp(stmmac_get_syncdevicetime,
 230                                              priv, NULL, xtstamp);
 231 }
 232
 233 /* structure describing a PTP hardware clock */
 234 static struct ptp_clock_info stmmac_ptp_clock_ops = {
 235         .owner = THIS_MODULE,
 236         .name = "stmmac ptp",
 237         .max_adj = 62500000,
 238         .n_alarm = 0,
 239         .n_ext_ts = 0, /* will be overwritten in stmmac_ptp_register */
 240         .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */
 241         .n_pins = 0,
 242         .pps = 0,
 243         .adjfreq = stmmac_adjust_freq,
 244         .adjtime = stmmac_adjust_time,
 245         .gettime64 = stmmac_get_time,
 246         .settime64 = stmmac_set_time,
 247         .enable = stmmac_enable,
 248         .getcrosststamp = stmmac_getcrosststamp,
 249 };
 250
 251 /**
 252  * stmmac_ptp_register
 253  * @priv: driver private structure
 254  * Description: this function will register the ptp clock driver
 255  * to kernel. It also does some house keeping work.
 256  */
 257 void stmmac_ptp_register(struct stmmac_priv *priv)
 258 {
 259         int i;
 260
 261         if (priv->plat->ptp_clk_freq_config)
 262                 priv->plat->ptp_clk_freq_config(priv);
 263
 264         for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
 265                 if (i >= STMMAC_PPS_MAX)
 266                         break;
 267                 priv->pps[i].available = true;
 268         }
 269
 270         if (priv->plat->ptp_max_adj)
 271                 stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;
 272
 273         stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
 274         stmmac_ptp_clock_ops.n_ext_ts = priv->dma_cap.aux_snapshot_n;
 275
 276         spin_lock_init(&priv->ptp_lock);
 277         mutex_init(&priv->aux_ts_lock);
 278         priv->ptp_clock_ops = stmmac_ptp_clock_ops;
 279
 280         priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
 281                                              priv->device);
 282         if (IS_ERR(priv->ptp_clock)) {
 283                 netdev_err(priv->dev, "ptp_clock_register failed\n");
 284                 priv->ptp_clock = NULL;
 285         } else if (priv->ptp_clock)
 286                 netdev_info(priv->dev, "registered PTP clock\n");
 287 }
 288
 289 /**
 290  * stmmac_ptp_unregister
 291  * @priv: driver private structure
 292  * Description: this function will remove/unregister the ptp clock driver
 293  * from the kernel.
 294  */
 295 void stmmac_ptp_unregister(struct stmmac_priv *priv)
 296 {
 297         if (priv->ptp_clock) {
 298                 ptp_clock_unregister(priv->ptp_clock);
 299                 priv->ptp_clock = NULL;
 300                 pr_debug("Removed PTP HW clock successfully on %s\n",
 301                          priv->dev->name);
 302         }
 303
 304         mutex_destroy(&priv->aux_ts_lock);
 305 }