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 timespec *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 u32 reminder; 115 116 spin_lock_irqsave(&priv->ptp_lock, flags); 117 118 ns = priv->hw->ptp->get_systime(priv->ioaddr); 119 120 spin_unlock_irqrestore(&priv->ptp_lock, flags); 121 122 ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &reminder); 123 ts->tv_nsec = reminder; 124 125 return 0; 126 } 127 128 /** 129 * stmmac_set_time 130 * 131 * @ptp: pointer to ptp_clock_info structure 132 * @ts: time value to set 133 * 134 * Description: this function will set the current time on the 135 * hardware clock. 136 */ 137 static int stmmac_set_time(struct ptp_clock_info *ptp, 138 const struct timespec *ts) 139 { 140 struct stmmac_priv *priv = 141 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 142 unsigned long flags; 143 144 spin_lock_irqsave(&priv->ptp_lock, flags); 145 146 priv->hw->ptp->init_systime(priv->ioaddr, ts->tv_sec, ts->tv_nsec); 147 148 spin_unlock_irqrestore(&priv->ptp_lock, flags); 149 150 return 0; 151 } 152 153 static int stmmac_enable(struct ptp_clock_info *ptp, 154 struct ptp_clock_request *rq, int on) 155 { 156 return -EOPNOTSUPP; 157 } 158 159 /* structure describing a PTP hardware clock */ 160 static struct ptp_clock_info stmmac_ptp_clock_ops = { 161 .owner = THIS_MODULE, 162 .name = "stmmac_ptp_clock", 163 .max_adj = 62500000, 164 .n_alarm = 0, 165 .n_ext_ts = 0, 166 .n_per_out = 0, 167 .pps = 0, 168 .adjfreq = stmmac_adjust_freq, 169 .adjtime = stmmac_adjust_time, 170 .gettime = stmmac_get_time, 171 .settime = 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 int 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 priv->ptp_clock = NULL; 190 pr_err("ptp_clock_register() failed on %s\n", priv->dev->name); 191 } else 192 pr_debug("Added PTP HW clock successfully on %s\n", 193 priv->dev->name); 194 195 return 0; 196 } 197 198 /** 199 * stmmac_ptp_unregister 200 * @priv: driver private structure 201 * Description: this function will remove/unregister the ptp clock driver 202 * from the kernel. 203 */ 204 void stmmac_ptp_unregister(struct stmmac_priv *priv) 205 { 206 if (priv->ptp_clock) { 207 ptp_clock_unregister(priv->ptp_clock); 208 pr_debug("Removed PTP HW clock successfully on %s\n", 209 priv->dev->name); 210 } 211 } 212