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_clock", 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 } 210