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