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
  53         priv->hw->ptp->config_addend(priv->ptpaddr, addend);
  54
  55         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  56
  57         return 0;
  58 }
  59
  60 /**
  61  * stmmac_adjust_time
  62  *
  63  * @ptp: pointer to ptp_clock_info structure
  64  * @delta: desired change in nanoseconds
  65  *
  66  * Description: this function will shift/adjust the hardware clock time.
  67  */
  68 static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
  69 {
  70         struct stmmac_priv *priv =
  71             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
  72         unsigned long flags;
  73         u32 sec, nsec;
  74         u32 quotient, reminder;
  75         int neg_adj = 0;
  76
  77         if (delta < 0) {
  78                 neg_adj = 1;
  79                 delta = -delta;
  80         }
  81
  82         quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
  83         sec = quotient;
  84         nsec = reminder;
  85
  86         spin_lock_irqsave(&priv->ptp_lock, flags);
  87
  88         priv->hw->ptp->adjust_systime(priv->ptpaddr, sec, nsec, neg_adj,
  89                                       priv->plat->has_gmac4);
  90
  91         spin_unlock_irqrestore(&priv->ptp_lock, flags);
  92
  93         return 0;
  94 }
  95
  96 /**
  97  * stmmac_get_time
  98  *
  99  * @ptp: pointer to ptp_clock_info structure
 100  * @ts: pointer to hold time/result
 101  *
 102  * Description: this function will read the current time from the
 103  * hardware clock and store it in @ts.
 104  */
 105 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
 106 {
 107         struct stmmac_priv *priv =
 108             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 109         unsigned long flags;
 110         u64 ns;
 111
 112         spin_lock_irqsave(&priv->ptp_lock, flags);
 113
 114         ns = priv->hw->ptp->get_systime(priv->ptpaddr);
 115
 116         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 117
 118         *ts = ns_to_timespec64(ns);
 119
 120         return 0;
 121 }
 122
 123 /**
 124  * stmmac_set_time
 125  *
 126  * @ptp: pointer to ptp_clock_info structure
 127  * @ts: time value to set
 128  *
 129  * Description: this function will set the current time on the
 130  * hardware clock.
 131  */
 132 static int stmmac_set_time(struct ptp_clock_info *ptp,
 133                            const struct timespec64 *ts)
 134 {
 135         struct stmmac_priv *priv =
 136             container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 137         unsigned long flags;
 138
 139         spin_lock_irqsave(&priv->ptp_lock, flags);
 140
 141         priv->hw->ptp->init_systime(priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
 142
 143         spin_unlock_irqrestore(&priv->ptp_lock, flags);
 144
 145         return 0;
 146 }
 147
 148 static int stmmac_enable(struct ptp_clock_info *ptp,
 149                          struct ptp_clock_request *rq, int on)
 150 {
 151         return -EOPNOTSUPP;
 152 }
 153
 154 /* structure describing a PTP hardware clock */
 155 static const struct ptp_clock_info stmmac_ptp_clock_ops = {
 156         .owner = THIS_MODULE,
 157         .name = "stmmac_ptp_clock",
 158         .max_adj = 62500000,
 159         .n_alarm = 0,
 160         .n_ext_ts = 0,
 161         .n_per_out = 0,
 162         .n_pins = 0,
 163         .pps = 0,
 164         .adjfreq = stmmac_adjust_freq,
 165         .adjtime = stmmac_adjust_time,
 166         .gettime64 = stmmac_get_time,
 167         .settime64 = stmmac_set_time,
 168         .enable = stmmac_enable,
 169 };
 170
 171 /**
 172  * stmmac_ptp_register
 173  * @priv: driver private structure
 174  * Description: this function will register the ptp clock driver
 175  * to kernel. It also does some house keeping work.
 176  */
 177 void stmmac_ptp_register(struct stmmac_priv *priv)
 178 {
 179         spin_lock_init(&priv->ptp_lock);
 180         priv->ptp_clock_ops = stmmac_ptp_clock_ops;
 181
 182         priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
 183                                              priv->device);
 184         if (IS_ERR(priv->ptp_clock)) {
 185                 netdev_err(priv->dev, "ptp_clock_register failed\n");
 186                 priv->ptp_clock = NULL;
 187         } else if (priv->ptp_clock)
 188                 netdev_info(priv->dev, "registered PTP clock\n");
 189 }
 190
 191 /**
 192  * stmmac_ptp_unregister
 193  * @priv: driver private structure
 194  * Description: this function will remove/unregister the ptp clock driver
 195  * from the kernel.
 196  */
 197 void stmmac_ptp_unregister(struct stmmac_priv *priv)
 198 {
 199         if (priv->ptp_clock) {
 200                 ptp_clock_unregister(priv->ptp_clock);
 201                 priv->ptp_clock = NULL;
 202                 pr_debug("Removed PTP HW clock successfully on %s\n",
 203                          priv->dev->name);
 204         }
 205 }