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, est_rst = false;
  66         int ret;
  67
  68         xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;
  69
  70         if (delta < 0) {
  71                 neg_adj = 1;
  72                 delta = -delta;
  73         }
  74
  75         quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
  76         sec = quotient;
  77         nsec = reminder;
  78
  79         /* If EST is enabled, disabled it before adjust ptp time. */
  80         if (priv->plat->est && priv->plat->est->enable) {
  81                 est_rst = true;
  82                 mutex_lock(&priv->plat->est->lock);
  83                 priv->plat->est->enable = false;
  84                 stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
  85                                      priv->plat->clk_ptp_rate);
  86                 mutex_unlock(&priv->plat->est->lock);
  87         }
  88
  89         spin_lock_irqsave(&priv->ptp_lock, flags);
  90         stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
  91         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  92
  93         /* Caculate new basetime and re-configured EST after PTP time adjust. */
  94         if (est_rst) {
  95                 struct timespec64 current_time, time;
  96                 ktime_t current_time_ns, basetime;
  97                 u64 cycle_time;
  98
  99                 mutex_lock(&priv->plat->est->lock);
 100                 priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, &current_time);
 101                 current_time_ns = timespec64_to_ktime(current_time);
 102                 time.tv_nsec = priv->plat->est->btr_reserve[0];
 103                 time.tv_sec = priv->plat->est->btr_reserve[1];
 104                 basetime = timespec64_to_ktime(time);
 105                 cycle_time = priv->plat->est->ctr[1] * NSEC_PER_SEC +
 106                              priv->plat->est->ctr[0];
 107                 time = stmmac_calc_tas_basetime(basetime,
 108                                                 current_time_ns,
 109                                                 cycle_time);
 110
 111                 priv->plat->est->btr[0] = (u32)time.tv_nsec;
 112                 priv->plat->est->btr[1] = (u32)time.tv_sec;
 113                 priv->plat->est->enable = true;
 114                 ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
 115                                            priv->plat->clk_ptp_rate);
 116                 mutex_unlock(&priv->plat->est->lock);
 117                 if (ret)
 118                         netdev_err(priv->dev, "failed to configure EST\n");
 119         }
 120
 121         return 0;
 122 }
 123
 124 /**
 125  * stmmac_get_time
 126  *
 127  * @ptp: pointer to ptp_clock_info structure
 128  * @ts: pointer to hold time/result
 129  *
 130  * Description: this function will read the current time from the
 131  * hardware clock and store it in @ts.
 132  */
 133 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
 134 {
 135         struct stmmac_priv *priv =
 136             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 137         unsigned long flags;
 138         u64 ns = 0;
 139
 140         spin_lock_irqsave(&priv->ptp_lock, flags);
 141         stmmac_get_systime(priv, priv->ptpaddr, &ns);
 142         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 143
 144         *ts = ns_to_timespec64(ns);
 145
 146         return 0;
 147 }
 148
 149 /**
 150  * stmmac_set_time
 151  *
 152  * @ptp: pointer to ptp_clock_info structure
 153  * @ts: time value to set
 154  *
 155  * Description: this function will set the current time on the
 156  * hardware clock.
 157  */
 158 static int stmmac_set_time(struct ptp_clock_info *ptp,
 159                            const struct timespec64 *ts)
 160 {
 161         struct stmmac_priv *priv =
 162             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 163         unsigned long flags;
 164
 165         spin_lock_irqsave(&priv->ptp_lock, flags);
 166         stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
 167         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 168
 169         return 0;
 170 }
 171
 172 static int stmmac_enable(struct ptp_clock_info *ptp,
 173                          struct ptp_clock_request *rq, int on)
 174 {
 175         struct stmmac_priv *priv =
 176             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 177         void __iomem *ptpaddr = priv->ptpaddr;
 178         void __iomem *ioaddr = priv->hw->pcsr;
 179         struct stmmac_pps_cfg *cfg;
 180         u32 intr_value, acr_value;
 181         int ret = -EOPNOTSUPP;
 182         unsigned long flags;
 183
 184         switch (rq->type) {
 185         case PTP_CLK_REQ_PEROUT:
 186                 /* Reject requests with unsupported flags */
 187                 if (rq->perout.flags)
 188                         return -EOPNOTSUPP;
 189
 190                 cfg = &priv->pps[rq->perout.index];
 191
 192                 cfg->start.tv_sec = rq->perout.start.sec;
 193                 cfg->start.tv_nsec = rq->perout.start.nsec;
 194                 cfg->period.tv_sec = rq->perout.period.sec;
 195                 cfg->period.tv_nsec = rq->perout.period.nsec;
 196
 197                 spin_lock_irqsave(&priv->ptp_lock, flags);
 198                 ret = stmmac_flex_pps_config(priv, priv->ioaddr,
 199                                              rq->perout.index, cfg, on,
 200                                              priv->sub_second_inc,
 201                                              priv->systime_flags);
 202                 spin_unlock_irqrestore(&priv->ptp_lock, flags);
 203                 break;
 204         case PTP_CLK_REQ_EXTTS:
 205                 priv->plat->ext_snapshot_en = on;
 206                 mutex_lock(&priv->aux_ts_lock);
 207                 acr_value = readl(ptpaddr + PTP_ACR);
 208                 acr_value &= ~PTP_ACR_MASK;
 209                 if (on) {
 210                         /* Enable External snapshot trigger */
 211                         acr_value |= priv->plat->ext_snapshot_num;
 212                         acr_value |= PTP_ACR_ATSFC;
 213                         netdev_dbg(priv->dev, "Auxiliary Snapshot %d enabled.\n",
 214                                    priv->plat->ext_snapshot_num >>
 215                                    PTP_ACR_ATSEN_SHIFT);
 216                         /* Enable Timestamp Interrupt */
 217                         intr_value = readl(ioaddr + GMAC_INT_EN);
 218                         intr_value |= GMAC_INT_TSIE;
 219                         writel(intr_value, ioaddr + GMAC_INT_EN);
 220
 221                 } else {
 222                         netdev_dbg(priv->dev, "Auxiliary Snapshot %d disabled.\n",
 223                                    priv->plat->ext_snapshot_num >>
 224                                    PTP_ACR_ATSEN_SHIFT);
 225                         /* Disable Timestamp Interrupt */
 226                         intr_value = readl(ioaddr + GMAC_INT_EN);
 227                         intr_value &= ~GMAC_INT_TSIE;
 228                         writel(intr_value, ioaddr + GMAC_INT_EN);
 229                 }
 230                 writel(acr_value, ptpaddr + PTP_ACR);
 231                 mutex_unlock(&priv->aux_ts_lock);
 232                 ret = 0;
 233                 break;
 234
 235         default:
 236                 break;
 237         }
 238
 239         return ret;
 240 }
 241
 242 /**
 243  * stmmac_get_syncdevicetime
 244  * @device: current device time
 245  * @system: system counter value read synchronously with device time
 246  * @ctx: context provided by timekeeping code
 247  * Description: Read device and system clock simultaneously and return the
 248  * corrected clock values in ns.
 249  **/
 250 static int stmmac_get_syncdevicetime(ktime_t *device,
 251                                      struct system_counterval_t *system,
 252                                      void *ctx)
 253 {
 254         struct stmmac_priv *priv = (struct stmmac_priv *)ctx;
 255
 256         if (priv->plat->crosststamp)
 257                 return priv->plat->crosststamp(device, system, ctx);
 258         else
 259                 return -EOPNOTSUPP;
 260 }
 261
 262 static int stmmac_getcrosststamp(struct ptp_clock_info *ptp,
 263                                  struct system_device_crosststamp *xtstamp)
 264 {
 265         struct stmmac_priv *priv =
 266                 container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 267
 268         return get_device_system_crosststamp(stmmac_get_syncdevicetime,
 269                                              priv, NULL, xtstamp);
 270 }
 271
 272 /* structure describing a PTP hardware clock */
 273 static struct ptp_clock_info stmmac_ptp_clock_ops = {
 274         .owner = THIS_MODULE,
 275         .name = "stmmac ptp",
 276         .max_adj = 62500000,
 277         .n_alarm = 0,
 278         .n_ext_ts = 0, /* will be overwritten in stmmac_ptp_register */
 279         .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */
 280         .n_pins = 0,
 281         .pps = 0,
 282         .adjfreq = stmmac_adjust_freq,
 283         .adjtime = stmmac_adjust_time,
 284         .gettime64 = stmmac_get_time,
 285         .settime64 = stmmac_set_time,
 286         .enable = stmmac_enable,
 287         .getcrosststamp = stmmac_getcrosststamp,
 288 };
 289
 290 /**
 291  * stmmac_ptp_register
 292  * @priv: driver private structure
 293  * Description: this function will register the ptp clock driver
 294  * to kernel. It also does some house keeping work.
 295  */
 296 void stmmac_ptp_register(struct stmmac_priv *priv)
 297 {
 298         int i;
 299
 300         if (priv->plat->ptp_clk_freq_config)
 301                 priv->plat->ptp_clk_freq_config(priv);
 302
 303         for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
 304                 if (i >= STMMAC_PPS_MAX)
 305                         break;
 306                 priv->pps[i].available = true;
 307         }
 308
 309         if (priv->plat->ptp_max_adj)
 310                 stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;
 311
 312         stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
 313         stmmac_ptp_clock_ops.n_ext_ts = priv->dma_cap.aux_snapshot_n;
 314
 315         spin_lock_init(&priv->ptp_lock);
 316         mutex_init(&priv->aux_ts_lock);
 317         priv->ptp_clock_ops = stmmac_ptp_clock_ops;
 318
 319         priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
 320                                              priv->device);
 321         if (IS_ERR(priv->ptp_clock)) {
 322                 netdev_err(priv->dev, "ptp_clock_register failed\n");
 323                 priv->ptp_clock = NULL;
 324         } else if (priv->ptp_clock)
 325                 netdev_info(priv->dev, "registered PTP clock\n");
 326 }
 327
 328 /**
 329  * stmmac_ptp_unregister
 330  * @priv: driver private structure
 331  * Description: this function will remove/unregister the ptp clock driver
 332  * from the kernel.
 333  */
 334 void stmmac_ptp_unregister(struct stmmac_priv *priv)
 335 {
 336         if (priv->ptp_clock) {
 337                 ptp_clock_unregister(priv->ptp_clock);
 338                 priv->ptp_clock = NULL;
 339                 pr_debug("Removed PTP HW clock successfully on %s\n",
 340                          priv->dev->name);
 341         }
 342
 343         mutex_destroy(&priv->aux_ts_lock);
 344 }