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 bool xmac; 75 76 xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac; 77 78 if (delta < 0) { 79 neg_adj = 1; 80 delta = -delta; 81 } 82 83 quotient = div_u64_rem(delta, 1000000000ULL, &reminder); 84 sec = quotient; 85 nsec = reminder; 86 87 spin_lock_irqsave(&priv->ptp_lock, flags); 88 stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac); 89 spin_unlock_irqrestore(&priv->ptp_lock, flags); 90 91 return 0; 92 } 93 94 /** 95 * stmmac_get_time 96 * 97 * @ptp: pointer to ptp_clock_info structure 98 * @ts: pointer to hold time/result 99 * 100 * Description: this function will read the current time from the 101 * hardware clock and store it in @ts. 102 */ 103 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts) 104 { 105 struct stmmac_priv *priv = 106 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 107 unsigned long flags; 108 u64 ns = 0; 109 110 spin_lock_irqsave(&priv->ptp_lock, flags); 111 stmmac_get_systime(priv, priv->ptpaddr, &ns); 112 spin_unlock_irqrestore(&priv->ptp_lock, flags); 113 114 *ts = ns_to_timespec64(ns); 115 116 return 0; 117 } 118 119 /** 120 * stmmac_set_time 121 * 122 * @ptp: pointer to ptp_clock_info structure 123 * @ts: time value to set 124 * 125 * Description: this function will set the current time on the 126 * hardware clock. 127 */ 128 static int stmmac_set_time(struct ptp_clock_info *ptp, 129 const struct timespec64 *ts) 130 { 131 struct stmmac_priv *priv = 132 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 133 unsigned long flags; 134 135 spin_lock_irqsave(&priv->ptp_lock, flags); 136 stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec); 137 spin_unlock_irqrestore(&priv->ptp_lock, flags); 138 139 return 0; 140 } 141 142 static int stmmac_enable(struct ptp_clock_info *ptp, 143 struct ptp_clock_request *rq, int on) 144 { 145 struct stmmac_priv *priv = 146 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 147 struct stmmac_pps_cfg *cfg; 148 int ret = -EOPNOTSUPP; 149 unsigned long flags; 150 151 switch (rq->type) { 152 case PTP_CLK_REQ_PEROUT: 153 cfg = &priv->pps[rq->perout.index]; 154 155 cfg->start.tv_sec = rq->perout.start.sec; 156 cfg->start.tv_nsec = rq->perout.start.nsec; 157 cfg->period.tv_sec = rq->perout.period.sec; 158 cfg->period.tv_nsec = rq->perout.period.nsec; 159 160 spin_lock_irqsave(&priv->ptp_lock, flags); 161 ret = stmmac_flex_pps_config(priv, priv->ioaddr, 162 rq->perout.index, cfg, on, 163 priv->sub_second_inc, 164 priv->systime_flags); 165 spin_unlock_irqrestore(&priv->ptp_lock, flags); 166 break; 167 default: 168 break; 169 } 170 171 return ret; 172 } 173 174 /* structure describing a PTP hardware clock */ 175 static struct ptp_clock_info stmmac_ptp_clock_ops = { 176 .owner = THIS_MODULE, 177 .name = "stmmac_ptp_clock", 178 .max_adj = 62500000, 179 .n_alarm = 0, 180 .n_ext_ts = 0, 181 .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */ 182 .n_pins = 0, 183 .pps = 0, 184 .adjfreq = stmmac_adjust_freq, 185 .adjtime = stmmac_adjust_time, 186 .gettime64 = stmmac_get_time, 187 .settime64 = stmmac_set_time, 188 .enable = stmmac_enable, 189 }; 190 191 /** 192 * stmmac_ptp_register 193 * @priv: driver private structure 194 * Description: this function will register the ptp clock driver 195 * to kernel. It also does some house keeping work. 196 */ 197 void stmmac_ptp_register(struct stmmac_priv *priv) 198 { 199 int i; 200 201 for (i = 0; i < priv->dma_cap.pps_out_num; i++) { 202 if (i >= STMMAC_PPS_MAX) 203 break; 204 priv->pps[i].available = true; 205 } 206 207 stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num; 208 209 spin_lock_init(&priv->ptp_lock); 210 priv->ptp_clock_ops = stmmac_ptp_clock_ops; 211 212 priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops, 213 priv->device); 214 if (IS_ERR(priv->ptp_clock)) { 215 netdev_err(priv->dev, "ptp_clock_register failed\n"); 216 priv->ptp_clock = NULL; 217 } else if (priv->ptp_clock) 218 netdev_info(priv->dev, "registered PTP clock\n"); 219 } 220 221 /** 222 * stmmac_ptp_unregister 223 * @priv: driver private structure 224 * Description: this function will remove/unregister the ptp clock driver 225 * from the kernel. 226 */ 227 void stmmac_ptp_unregister(struct stmmac_priv *priv) 228 { 229 if (priv->ptp_clock) { 230 ptp_clock_unregister(priv->ptp_clock); 231 priv->ptp_clock = NULL; 232 pr_debug("Removed PTP HW clock successfully on %s\n", 233 priv->dev->name); 234 } 235 } 236