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 timespec *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         u32 reminder;
 115
 116         spin_lock_irqsave(&priv->ptp_lock, flags);
 117
 118         ns = priv->hw->ptp->get_systime(priv->ioaddr);
 119
 120         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 121
 122         ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &reminder);
 123         ts->tv_nsec = reminder;
 124
 125         return 0;
 126 }
 127
 128 /**
 129  * stmmac_set_time
 130  *
 131  * @ptp: pointer to ptp_clock_info structure
 132  * @ts: time value to set
 133  *
 134  * Description: this function will set the current time on the
 135  * hardware clock.
 136  */
 137 static int stmmac_set_time(struct ptp_clock_info *ptp,
 138                            const struct timespec *ts)
 139 {
 140         struct stmmac_priv *priv =
 141             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 142         unsigned long flags;
 143
 144         spin_lock_irqsave(&priv->ptp_lock, flags);
 145
 146         priv->hw->ptp->init_systime(priv->ioaddr, ts->tv_sec, ts->tv_nsec);
 147
 148         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 149
 150         return 0;
 151 }
 152
 153 static int stmmac_enable(struct ptp_clock_info *ptp,
 154                          struct ptp_clock_request *rq, int on)
 155 {
 156         return -EOPNOTSUPP;
 157 }
 158
 159 /* structure describing a PTP hardware clock */
 160 static struct ptp_clock_info stmmac_ptp_clock_ops = {
 161         .owner = THIS_MODULE,
 162         .name = "stmmac_ptp_clock",
 163         .max_adj = 62500000,
 164         .n_alarm = 0,
 165         .n_ext_ts = 0,
 166         .n_per_out = 0,
 167         .pps = 0,
 168         .adjfreq = stmmac_adjust_freq,
 169         .adjtime = stmmac_adjust_time,
 170         .gettime = stmmac_get_time,
 171         .settime = stmmac_set_time,
 172         .enable = stmmac_enable,
 173 };
 174
 175 /**
 176  * stmmac_ptp_register
 177  * @priv: driver private structure
 178  * Description: this function will register the ptp clock driver
 179  * to kernel. It also does some house keeping work.
 180  */
 181 int stmmac_ptp_register(struct stmmac_priv *priv)
 182 {
 183         spin_lock_init(&priv->ptp_lock);
 184         priv->ptp_clock_ops = stmmac_ptp_clock_ops;
 185
 186         priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
 187                                              priv->device);
 188         if (IS_ERR(priv->ptp_clock)) {
 189                 priv->ptp_clock = NULL;
 190                 pr_err("ptp_clock_register() failed on %s\n", priv->dev->name);
 191         } else
 192                 pr_debug("Added PTP HW clock successfully on %s\n",
 193                          priv->dev->name);
 194
 195         return 0;
 196 }
 197
 198 /**
 199  * stmmac_ptp_unregister
 200  * @priv: driver private structure
 201  * Description: this function will remove/unregister the ptp clock driver
 202  * from the kernel.
 203  */
 204 void stmmac_ptp_unregister(struct stmmac_priv *priv)
 205 {
 206         if (priv->ptp_clock) {
 207                 ptp_clock_unregister(priv->ptp_clock);
 208                 pr_debug("Removed PTP HW clock successfully on %s\n",
 209                          priv->dev->name);
 210         }
 211 }