diff options
Diffstat (limited to 'drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c')
| -rw-r--r-- | drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c | 211 |
1 files changed, 211 insertions, 0 deletions
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c new file mode 100644 index 000000000..170a18b61 --- /dev/null +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c @@ -0,0 +1,211 @@ +/******************************************************************************* + PTP 1588 clock using the STMMAC. + + Copyright (C) 2013 Vayavya Labs Pvt Ltd + + This program is free software; you can redistribute it and/or modify it + under the terms and conditions of the GNU General Public License, + version 2, as published by the Free Software Foundation. + + This program is distributed in the hope it will be useful, but WITHOUT + ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + more details. + + You should have received a copy of the GNU General Public License along with + this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. + + The full GNU General Public License is included in this distribution in + the file called "COPYING". + + Author: Rayagond Kokatanur <rayagond@vayavyalabs.com> +*******************************************************************************/ +#include "stmmac.h" +#include "stmmac_ptp.h" + +/** + * stmmac_adjust_freq + * + * @ptp: pointer to ptp_clock_info structure + * @ppb: desired period change in parts ber billion + * + * Description: this function will adjust the frequency of hardware clock. + */ +static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb) +{ + struct stmmac_priv *priv = + container_of(ptp, struct stmmac_priv, ptp_clock_ops); + unsigned long flags; + u32 diff, addend; + int neg_adj = 0; + u64 adj; + + if (ppb < 0) { + neg_adj = 1; + ppb = -ppb; + } + + addend = priv->default_addend; + adj = addend; + adj *= ppb; + diff = div_u64(adj, 1000000000ULL); + addend = neg_adj ? (addend - diff) : (addend + diff); + + spin_lock_irqsave(&priv->ptp_lock, flags); + + priv->hw->ptp->config_addend(priv->ioaddr, addend); + + spin_unlock_irqrestore(&priv->ptp_lock, flags); + + return 0; +} + +/** + * stmmac_adjust_time + * + * @ptp: pointer to ptp_clock_info structure + * @delta: desired change in nanoseconds + * + * Description: this function will shift/adjust the hardware clock time. + */ +static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta) +{ + struct stmmac_priv *priv = + container_of(ptp, struct stmmac_priv, ptp_clock_ops); + unsigned long flags; + u32 sec, nsec; + u32 quotient, reminder; + int neg_adj = 0; + + if (delta < 0) { + neg_adj = 1; + delta = -delta; + } + + quotient = div_u64_rem(delta, 1000000000ULL, &reminder); + sec = quotient; + nsec = reminder; + + spin_lock_irqsave(&priv->ptp_lock, flags); + + priv->hw->ptp->adjust_systime(priv->ioaddr, sec, nsec, neg_adj); + + spin_unlock_irqrestore(&priv->ptp_lock, flags); + + return 0; +} + +/** + * stmmac_get_time + * + * @ptp: pointer to ptp_clock_info structure + * @ts: pointer to hold time/result + * + * Description: this function will read the current time from the + * hardware clock and store it in @ts. + */ +static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts) +{ + struct stmmac_priv *priv = + container_of(ptp, struct stmmac_priv, ptp_clock_ops); + unsigned long flags; + u64 ns; + + spin_lock_irqsave(&priv->ptp_lock, flags); + + ns = priv->hw->ptp->get_systime(priv->ioaddr); + + spin_unlock_irqrestore(&priv->ptp_lock, flags); + + *ts = ns_to_timespec64(ns); + + return 0; +} + +/** + * stmmac_set_time + * + * @ptp: pointer to ptp_clock_info structure + * @ts: time value to set + * + * Description: this function will set the current time on the + * hardware clock. + */ +static int stmmac_set_time(struct ptp_clock_info *ptp, + const struct timespec64 *ts) +{ + struct stmmac_priv *priv = + container_of(ptp, struct stmmac_priv, ptp_clock_ops); + unsigned long flags; + + spin_lock_irqsave(&priv->ptp_lock, flags); + + priv->hw->ptp->init_systime(priv->ioaddr, ts->tv_sec, ts->tv_nsec); + + spin_unlock_irqrestore(&priv->ptp_lock, flags); + + return 0; +} + +static int stmmac_enable(struct ptp_clock_info *ptp, + struct ptp_clock_request *rq, int on) +{ + return -EOPNOTSUPP; +} + +/* structure describing a PTP hardware clock */ +static struct ptp_clock_info stmmac_ptp_clock_ops = { + .owner = THIS_MODULE, + .name = "stmmac_ptp_clock", + .max_adj = 62500000, + .n_alarm = 0, + .n_ext_ts = 0, + .n_per_out = 0, + .n_pins = 0, + .pps = 0, + .adjfreq = stmmac_adjust_freq, + .adjtime = stmmac_adjust_time, + .gettime64 = stmmac_get_time, + .settime64 = stmmac_set_time, + .enable = stmmac_enable, +}; + +/** + * stmmac_ptp_register + * @priv: driver private structure + * Description: this function will register the ptp clock driver + * to kernel. It also does some house keeping work. + */ +int stmmac_ptp_register(struct stmmac_priv *priv) +{ + spin_lock_init(&priv->ptp_lock); + priv->ptp_clock_ops = stmmac_ptp_clock_ops; + + priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops, + priv->device); + if (IS_ERR(priv->ptp_clock)) { + priv->ptp_clock = NULL; + pr_err("ptp_clock_register() failed on %s\n", priv->dev->name); + } else + pr_debug("Added PTP HW clock successfully on %s\n", + priv->dev->name); + + return 0; +} + +/** + * stmmac_ptp_unregister + * @priv: driver private structure + * Description: this function will remove/unregister the ptp clock driver + * from the kernel. + */ +void stmmac_ptp_unregister(struct stmmac_priv *priv) +{ + if (priv->ptp_clock) { + ptp_clock_unregister(priv->ptp_clock); + priv->ptp_clock = NULL; + pr_debug("Removed PTP HW clock successfully on %s\n", + priv->dev->name); + } +} |