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->ptpaddr, 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->ptpaddr, sec, nsec, neg_adj,
  93                                       priv->plat->has_gmac4);
  94
  95         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  96
  97         return 0;
  98 }
  99
 100 /**
 101  * stmmac_get_time
 102  *
 103  * @ptp: pointer to ptp_clock_info structure
 104  * @ts: pointer to hold time/result
 105  *
 106  * Description: this function will read the current time from the
 107  * hardware clock and store it in @ts.
 108  */
 109 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
 110 {
 111         struct stmmac_priv *priv =
 112             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 113         unsigned long flags;
 114         u64 ns;
 115
 116         spin_lock_irqsave(&priv->ptp_lock, flags);
 117
 118         ns = priv->hw->ptp->get_systime(priv->ptpaddr);
 119
 120         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 121
 122         *ts = ns_to_timespec64(ns);
 123
 124         return 0;
 125 }
 126
 127 /**
 128  * stmmac_set_time
 129  *
 130  * @ptp: pointer to ptp_clock_info structure
 131  * @ts: time value to set
 132  *
 133  * Description: this function will set the current time on the
 134  * hardware clock.
 135  */
 136 static int stmmac_set_time(struct ptp_clock_info *ptp,
 137                            const struct timespec64 *ts)
 138 {
 139         struct stmmac_priv *priv =
 140             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 141         unsigned long flags;
 142
 143         spin_lock_irqsave(&priv->ptp_lock, flags);
 144
 145         priv->hw->ptp->init_systime(priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
 146
 147         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 148
 149         return 0;
 150 }
 151
 152 static int stmmac_enable(struct ptp_clock_info *ptp,
 153                          struct ptp_clock_request *rq, int on)
 154 {
 155         return -EOPNOTSUPP;
 156 }
 157
 158 /* structure describing a PTP hardware clock */
 159 static struct ptp_clock_info stmmac_ptp_clock_ops = {
 160         .owner = THIS_MODULE,
 161         .name = "stmmac ptp",
 162         .max_adj = 62500000,
 163         .n_alarm = 0,
 164         .n_ext_ts = 0,
 165         .n_per_out = 0,
 166         .n_pins = 0,
 167         .pps = 0,
 168         .adjfreq = stmmac_adjust_freq,
 169         .adjtime = stmmac_adjust_time,
 170         .gettime64 = stmmac_get_time,
 171         .settime64 = 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 void 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                 netdev_err(priv->dev, "ptp_clock_register failed\n");
 190                 priv->ptp_clock = NULL;
 191         } else if (priv->ptp_clock)
 192                 netdev_info(priv->dev, "registered PTP clock\n");
 193 }
 194
 195 /**
 196  * stmmac_ptp_unregister
 197  * @priv: driver private structure
 198  * Description: this function will remove/unregister the ptp clock driver
 199  * from the kernel.
 200  */
 201 void stmmac_ptp_unregister(struct stmmac_priv *priv)
 202 {
 203         if (priv->ptp_clock) {
 204                 ptp_clock_unregister(priv->ptp_clock);
 205                 priv->ptp_clock = NULL;
 206                 pr_debug("Removed PTP HW clock successfully on %s\n",
 207                          priv->dev->name);
 208         }
 209 }