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

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