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   You should have received a copy of the GNU General Public License along with
  16   this program; if not, write to the Free Software Foundation, Inc.,
  17   51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
  18
  19   The full GNU General Public License is included in this distribution in
  20   the file called "COPYING".
  21
  22   Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
  23 *******************************************************************************/
  24 #include "stmmac.h"
  25 #include "stmmac_ptp.h"
  26
  27 /**
  28  * stmmac_adjust_freq
  29  *
  30  * @ptp: pointer to ptp_clock_info structure
  31  * @ppb: desired period change in parts ber billion
  32  *
  33  * Description: this function will adjust the frequency of hardware clock.
  34  */
  35 static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
  36 {
  37         struct stmmac_priv *priv =
  38             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
  39         unsigned long flags;
  40         u32 diff, addend;
  41         int neg_adj = 0;
  42         u64 adj;
  43
  44         if (ppb < 0) {
  45                 neg_adj = 1;
  46                 ppb = -ppb;
  47         }
  48
  49         addend = priv->default_addend;
  50         adj = addend;
  51         adj *= ppb;
  52         diff = div_u64(adj, 1000000000ULL);
  53         addend = neg_adj ? (addend - diff) : (addend + diff);
  54
  55         spin_lock_irqsave(&priv->ptp_lock, flags);
  56
  57         priv->hw->ptp->config_addend(priv->ioaddr, addend);
  58
  59         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  60
  61         return 0;
  62 }
  63
  64 /**
  65  * stmmac_adjust_time
  66  *
  67  * @ptp: pointer to ptp_clock_info structure
  68  * @delta: desired change in nanoseconds
  69  *
  70  * Description: this function will shift/adjust the hardware clock time.
  71  */
  72 static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
  73 {
  74         struct stmmac_priv *priv =
  75             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
  76         unsigned long flags;
  77         u32 sec, nsec;
  78         u32 quotient, reminder;
  79         int neg_adj = 0;
  80
  81         if (delta < 0) {
  82                 neg_adj = 1;
  83                 delta = -delta;
  84         }
  85
  86         quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
  87         sec = quotient;
  88         nsec = reminder;
  89
  90         spin_lock_irqsave(&priv->ptp_lock, flags);
  91
  92         priv->hw->ptp->adjust_systime(priv->ioaddr, sec, nsec, neg_adj);
  93
  94         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  95
  96         return 0;
  97 }
  98
  99 /**
 100  * stmmac_get_time
 101  *
 102  * @ptp: pointer to ptp_clock_info structure
 103  * @ts: pointer to hold time/result
 104  *
 105  * Description: this function will read the current time from the
 106  * hardware clock and store it in @ts.
 107  */
 108 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
 109 {
 110         struct stmmac_priv *priv =
 111             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 112         unsigned long flags;
 113         u64 ns;
 114
 115         spin_lock_irqsave(&priv->ptp_lock, flags);
 116
 117         ns = priv->hw->ptp->get_systime(priv->ioaddr);
 118
 119         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 120
 121         *ts = ns_to_timespec64(ns);
 122
 123         return 0;
 124 }
 125
 126 /**
 127  * stmmac_set_time
 128  *
 129  * @ptp: pointer to ptp_clock_info structure
 130  * @ts: time value to set
 131  *
 132  * Description: this function will set the current time on the
 133  * hardware clock.
 134  */
 135 static int stmmac_set_time(struct ptp_clock_info *ptp,
 136                            const struct timespec64 *ts)
 137 {
 138         struct stmmac_priv *priv =
 139             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 140         unsigned long flags;
 141
 142         spin_lock_irqsave(&priv->ptp_lock, flags);
 143
 144         priv->hw->ptp->init_systime(priv->ioaddr, ts->tv_sec, ts->tv_nsec);
 145
 146         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 147
 148         return 0;
 149 }
 150
 151 static int stmmac_enable(struct ptp_clock_info *ptp,
 152                          struct ptp_clock_request *rq, int on)
 153 {
 154         return -EOPNOTSUPP;
 155 }
 156
 157 /* structure describing a PTP hardware clock */
 158 static struct ptp_clock_info stmmac_ptp_clock_ops = {
 159         .owner = THIS_MODULE,
 160         .name = "stmmac_ptp_clock",
 161         .max_adj = 62500000,
 162         .n_alarm = 0,
 163         .n_ext_ts = 0,
 164         .n_per_out = 0,
 165         .n_pins = 0,
 166         .pps = 0,
 167         .adjfreq = stmmac_adjust_freq,
 168         .adjtime = stmmac_adjust_time,
 169         .gettime64 = stmmac_get_time,
 170         .settime64 = stmmac_set_time,
 171         .enable = stmmac_enable,
 172 };
 173
 174 /**
 175  * stmmac_ptp_register
 176  * @priv: driver private structure
 177  * Description: this function will register the ptp clock driver
 178  * to kernel. It also does some house keeping work.
 179  */
 180 int stmmac_ptp_register(struct stmmac_priv *priv)
 181 {
 182         spin_lock_init(&priv->ptp_lock);
 183         priv->ptp_clock_ops = stmmac_ptp_clock_ops;
 184
 185         priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
 186                                              priv->device);
 187         if (IS_ERR(priv->ptp_clock)) {
 188                 priv->ptp_clock = NULL;
 189                 pr_err("ptp_clock_register() failed on %s\n", priv->dev->name);
 190         } else
 191                 pr_debug("Added PTP HW clock successfully on %s\n",
 192                          priv->dev->name);
 193
 194         return 0;
 195 }
 196
 197 /**
 198  * stmmac_ptp_unregister
 199  * @priv: driver private structure
 200  * Description: this function will remove/unregister the ptp clock driver
 201  * from the kernel.
 202  */
 203 void stmmac_ptp_unregister(struct stmmac_priv *priv)
 204 {
 205         if (priv->ptp_clock) {
 206                 ptp_clock_unregister(priv->ptp_clock);
 207                 priv->ptp_clock = NULL;
 208                 pr_debug("Removed PTP HW clock successfully on %s\n",
 209                          priv->dev->name);
 210         }
 211 }