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
  13 /**
  14  * stmmac_adjust_freq
  15  *
  16  * @ptp: pointer to ptp_clock_info structure
  17  * @ppb: desired period change in parts ber billion
  18  *
  19  * Description: this function will adjust the frequency of hardware clock.
  20  */
  21 static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
  22 {
  23         struct stmmac_priv *priv =
  24             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
  25         unsigned long flags;
  26         u32 diff, addend;
  27         int neg_adj = 0;
  28         u64 adj;
  29
  30         if (ppb < 0) {
  31                 neg_adj = 1;
  32                 ppb = -ppb;
  33         }
  34
  35         addend = priv->default_addend;
  36         adj = addend;
  37         adj *= ppb;
  38         diff = div_u64(adj, 1000000000ULL);
  39         addend = neg_adj ? (addend - diff) : (addend + diff);
  40
  41         spin_lock_irqsave(&priv->ptp_lock, flags);
  42         stmmac_config_addend(priv, priv->ptpaddr, addend);
  43         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  44
  45         return 0;
  46 }
  47
  48 /**
  49  * stmmac_adjust_time
  50  *
  51  * @ptp: pointer to ptp_clock_info structure
  52  * @delta: desired change in nanoseconds
  53  *
  54  * Description: this function will shift/adjust the hardware clock time.
  55  */
  56 static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
  57 {
  58         struct stmmac_priv *priv =
  59             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
  60         unsigned long flags;
  61         u32 sec, nsec;
  62         u32 quotient, reminder;
  63         int neg_adj = 0;
  64         bool xmac;
  65
  66         xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;
  67
  68         if (delta < 0) {
  69                 neg_adj = 1;
  70                 delta = -delta;
  71         }
  72
  73         quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
  74         sec = quotient;
  75         nsec = reminder;
  76
  77         spin_lock_irqsave(&priv->ptp_lock, flags);
  78         stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
  79         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  80
  81         return 0;
  82 }
  83
  84 /**
  85  * stmmac_get_time
  86  *
  87  * @ptp: pointer to ptp_clock_info structure
  88  * @ts: pointer to hold time/result
  89  *
  90  * Description: this function will read the current time from the
  91  * hardware clock and store it in @ts.
  92  */
  93 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
  94 {
  95         struct stmmac_priv *priv =
  96             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
  97         unsigned long flags;
  98         u64 ns = 0;
  99
 100         spin_lock_irqsave(&priv->ptp_lock, flags);
 101         stmmac_get_systime(priv, priv->ptpaddr, &ns);
 102         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 103
 104         *ts = ns_to_timespec64(ns);
 105
 106         return 0;
 107 }
 108
 109 /**
 110  * stmmac_set_time
 111  *
 112  * @ptp: pointer to ptp_clock_info structure
 113  * @ts: time value to set
 114  *
 115  * Description: this function will set the current time on the
 116  * hardware clock.
 117  */
 118 static int stmmac_set_time(struct ptp_clock_info *ptp,
 119                            const struct timespec64 *ts)
 120 {
 121         struct stmmac_priv *priv =
 122             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 123         unsigned long flags;
 124
 125         spin_lock_irqsave(&priv->ptp_lock, flags);
 126         stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
 127         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 128
 129         return 0;
 130 }
 131
 132 static int stmmac_enable(struct ptp_clock_info *ptp,
 133                          struct ptp_clock_request *rq, int on)
 134 {
 135         struct stmmac_priv *priv =
 136             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 137         struct stmmac_pps_cfg *cfg;
 138         int ret = -EOPNOTSUPP;
 139         unsigned long flags;
 140
 141         switch (rq->type) {
 142         case PTP_CLK_REQ_PEROUT:
 143                 /* Reject requests with unsupported flags */
 144                 if (rq->perout.flags)
 145                         return -EOPNOTSUPP;
 146
 147                 cfg = &priv->pps[rq->perout.index];
 148
 149                 cfg->start.tv_sec = rq->perout.start.sec;
 150                 cfg->start.tv_nsec = rq->perout.start.nsec;
 151                 cfg->period.tv_sec = rq->perout.period.sec;
 152                 cfg->period.tv_nsec = rq->perout.period.nsec;
 153
 154                 spin_lock_irqsave(&priv->ptp_lock, flags);
 155                 ret = stmmac_flex_pps_config(priv, priv->ioaddr,
 156                                              rq->perout.index, cfg, on,
 157                                              priv->sub_second_inc,
 158                                              priv->systime_flags);
 159                 spin_unlock_irqrestore(&priv->ptp_lock, flags);
 160                 break;
 161         default:
 162                 break;
 163         }
 164
 165         return ret;
 166 }
 167
 168 /* structure describing a PTP hardware clock */
 169 static struct ptp_clock_info stmmac_ptp_clock_ops = {
 170         .owner = THIS_MODULE,
 171         .name = "stmmac ptp",
 172         .max_adj = 62500000,
 173         .n_alarm = 0,
 174         .n_ext_ts = 0,
 175         .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */
 176         .n_pins = 0,
 177         .pps = 0,
 178         .adjfreq = stmmac_adjust_freq,
 179         .adjtime = stmmac_adjust_time,
 180         .gettime64 = stmmac_get_time,
 181         .settime64 = stmmac_set_time,
 182         .enable = stmmac_enable,
 183 };
 184
 185 /**
 186  * stmmac_ptp_register
 187  * @priv: driver private structure
 188  * Description: this function will register the ptp clock driver
 189  * to kernel. It also does some house keeping work.
 190  */
 191 void stmmac_ptp_register(struct stmmac_priv *priv)
 192 {
 193         int i;
 194
 195         for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
 196                 if (i >= STMMAC_PPS_MAX)
 197                         break;
 198                 priv->pps[i].available = true;
 199         }
 200
 201         if (priv->plat->ptp_max_adj)
 202                 stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;
 203
 204         stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
 205
 206         spin_lock_init(&priv->ptp_lock);
 207         priv->ptp_clock_ops = stmmac_ptp_clock_ops;
 208
 209         priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
 210                                              priv->device);
 211         if (IS_ERR(priv->ptp_clock)) {
 212                 netdev_err(priv->dev, "ptp_clock_register failed\n");
 213                 priv->ptp_clock = NULL;
 214         } else if (priv->ptp_clock)
 215                 netdev_info(priv->dev, "registered PTP clock\n");
 216 }
 217
 218 /**
 219  * stmmac_ptp_unregister
 220  * @priv: driver private structure
 221  * Description: this function will remove/unregister the ptp clock driver
 222  * from the kernel.
 223  */
 224 void stmmac_ptp_unregister(struct stmmac_priv *priv)
 225 {
 226         if (priv->ptp_clock) {
 227                 ptp_clock_unregister(priv->ptp_clock);
 228                 priv->ptp_clock = NULL;
 229                 pr_debug("Removed PTP HW clock successfully on %s\n",
 230                          priv->dev->name);
 231         }
 232 }