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         struct stmmac_pps_cfg *cfg;
 139         int ret = -EOPNOTSUPP;
 140         unsigned long flags;
 141
 142         switch (rq->type) {
 143         case PTP_CLK_REQ_PEROUT:
 144                 /* Reject requests with unsupported flags */
 145                 if (rq->perout.flags)
 146                         return -EOPNOTSUPP;
 147
 148                 cfg = &priv->pps[rq->perout.index];
 149
 150                 cfg->start.tv_sec = rq->perout.start.sec;
 151                 cfg->start.tv_nsec = rq->perout.start.nsec;
 152                 cfg->period.tv_sec = rq->perout.period.sec;
 153                 cfg->period.tv_nsec = rq->perout.period.nsec;
 154
 155                 spin_lock_irqsave(&priv->ptp_lock, flags);
 156                 ret = stmmac_flex_pps_config(priv, priv->ioaddr,
 157                                              rq->perout.index, cfg, on,
 158                                              priv->sub_second_inc,
 159                                              priv->systime_flags);
 160                 spin_unlock_irqrestore(&priv->ptp_lock, flags);
 161                 break;
 162         default:
 163                 break;
 164         }
 165
 166         return ret;
 167 }
 168
 169 /**
 170  * stmmac_get_syncdevicetime
 171  * @device: current device time
 172  * @system: system counter value read synchronously with device time
 173  * @ctx: context provided by timekeeping code
 174  * Description: Read device and system clock simultaneously and return the
 175  * corrected clock values in ns.
 176  **/
 177 static int stmmac_get_syncdevicetime(ktime_t *device,
 178                                      struct system_counterval_t *system,
 179                                      void *ctx)
 180 {
 181         struct stmmac_priv *priv = (struct stmmac_priv *)ctx;
 182
 183         if (priv->plat->crosststamp)
 184                 return priv->plat->crosststamp(device, system, ctx);
 185         else
 186                 return -EOPNOTSUPP;
 187 }
 188
 189 static int stmmac_getcrosststamp(struct ptp_clock_info *ptp,
 190                                  struct system_device_crosststamp *xtstamp)
 191 {
 192         struct stmmac_priv *priv =
 193                 container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 194
 195         return get_device_system_crosststamp(stmmac_get_syncdevicetime,
 196                                              priv, NULL, xtstamp);
 197 }
 198
 199 /* structure describing a PTP hardware clock */
 200 static struct ptp_clock_info stmmac_ptp_clock_ops = {
 201         .owner = THIS_MODULE,
 202         .name = "stmmac ptp",
 203         .max_adj = 62500000,
 204         .n_alarm = 0,
 205         .n_ext_ts = 0,
 206         .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */
 207         .n_pins = 0,
 208         .pps = 0,
 209         .adjfreq = stmmac_adjust_freq,
 210         .adjtime = stmmac_adjust_time,
 211         .gettime64 = stmmac_get_time,
 212         .settime64 = stmmac_set_time,
 213         .enable = stmmac_enable,
 214         .getcrosststamp = stmmac_getcrosststamp,
 215 };
 216
 217 /**
 218  * stmmac_ptp_register
 219  * @priv: driver private structure
 220  * Description: this function will register the ptp clock driver
 221  * to kernel. It also does some house keeping work.
 222  */
 223 void stmmac_ptp_register(struct stmmac_priv *priv)
 224 {
 225         int i;
 226
 227         if (priv->plat->ptp_clk_freq_config)
 228                 priv->plat->ptp_clk_freq_config(priv);
 229
 230         for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
 231                 if (i >= STMMAC_PPS_MAX)
 232                         break;
 233                 priv->pps[i].available = true;
 234         }
 235
 236         if (priv->plat->ptp_max_adj)
 237                 stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;
 238
 239         stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
 240
 241         spin_lock_init(&priv->ptp_lock);
 242         priv->ptp_clock_ops = stmmac_ptp_clock_ops;
 243
 244         priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
 245                                              priv->device);
 246         if (IS_ERR(priv->ptp_clock)) {
 247                 netdev_err(priv->dev, "ptp_clock_register failed\n");
 248                 priv->ptp_clock = NULL;
 249         } else if (priv->ptp_clock)
 250                 netdev_info(priv->dev, "registered PTP clock\n");
 251 }
 252
 253 /**
 254  * stmmac_ptp_unregister
 255  * @priv: driver private structure
 256  * Description: this function will remove/unregister the ptp clock driver
 257  * from the kernel.
 258  */
 259 void stmmac_ptp_unregister(struct stmmac_priv *priv)
 260 {
 261         if (priv->ptp_clock) {
 262                 ptp_clock_unregister(priv->ptp_clock);
 263                 priv->ptp_clock = NULL;
 264                 pr_debug("Removed PTP HW clock successfully on %s\n",
 265                          priv->dev->name);
 266         }
 267 }