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