1 // SPDX-License-Identifier: GPL-2.0-only 2 /******************************************************************************* 3 PTP 1588 clock using the STMMAC. 4 5 Copyright (C) 2013 Vayavya Labs Pvt Ltd 6 7 8 Author: Rayagond Kokatanur <rayagond@vayavyalabs.com> 9 *******************************************************************************/ 10 #include "stmmac.h" 11 #include "stmmac_ptp.h" 12 #include "dwmac4.h" 13 14 /** 15 * stmmac_adjust_freq 16 * 17 * @ptp: pointer to ptp_clock_info structure 18 * @ppb: desired period change in parts ber billion 19 * 20 * Description: this function will adjust the frequency of hardware clock. 21 */ 22 static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb) 23 { 24 struct stmmac_priv *priv = 25 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 26 unsigned long flags; 27 u32 diff, addend; 28 int neg_adj = 0; 29 u64 adj; 30 31 if (ppb < 0) { 32 neg_adj = 1; 33 ppb = -ppb; 34 } 35 36 addend = priv->default_addend; 37 adj = addend; 38 adj *= ppb; 39 diff = div_u64(adj, 1000000000ULL); 40 addend = neg_adj ? (addend - diff) : (addend + diff); 41 42 spin_lock_irqsave(&priv->ptp_lock, flags); 43 stmmac_config_addend(priv, priv->ptpaddr, addend); 44 spin_unlock_irqrestore(&priv->ptp_lock, flags); 45 46 return 0; 47 } 48 49 /** 50 * stmmac_adjust_time 51 * 52 * @ptp: pointer to ptp_clock_info structure 53 * @delta: desired change in nanoseconds 54 * 55 * Description: this function will shift/adjust the hardware clock time. 56 */ 57 static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta) 58 { 59 struct stmmac_priv *priv = 60 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 61 unsigned long flags; 62 u32 sec, nsec; 63 u32 quotient, reminder; 64 int neg_adj = 0; 65 bool xmac; 66 67 xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac; 68 69 if (delta < 0) { 70 neg_adj = 1; 71 delta = -delta; 72 } 73 74 quotient = div_u64_rem(delta, 1000000000ULL, &reminder); 75 sec = quotient; 76 nsec = reminder; 77 78 spin_lock_irqsave(&priv->ptp_lock, flags); 79 stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac); 80 spin_unlock_irqrestore(&priv->ptp_lock, flags); 81 82 return 0; 83 } 84 85 /** 86 * stmmac_get_time 87 * 88 * @ptp: pointer to ptp_clock_info structure 89 * @ts: pointer to hold time/result 90 * 91 * Description: this function will read the current time from the 92 * hardware clock and store it in @ts. 93 */ 94 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts) 95 { 96 struct stmmac_priv *priv = 97 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 98 unsigned long flags; 99 u64 ns = 0; 100 101 spin_lock_irqsave(&priv->ptp_lock, flags); 102 stmmac_get_systime(priv, priv->ptpaddr, &ns); 103 spin_unlock_irqrestore(&priv->ptp_lock, flags); 104 105 *ts = ns_to_timespec64(ns); 106 107 return 0; 108 } 109 110 /** 111 * stmmac_set_time 112 * 113 * @ptp: pointer to ptp_clock_info structure 114 * @ts: time value to set 115 * 116 * Description: this function will set the current time on the 117 * hardware clock. 118 */ 119 static int stmmac_set_time(struct ptp_clock_info *ptp, 120 const struct timespec64 *ts) 121 { 122 struct stmmac_priv *priv = 123 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 124 unsigned long flags; 125 126 spin_lock_irqsave(&priv->ptp_lock, flags); 127 stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec); 128 spin_unlock_irqrestore(&priv->ptp_lock, flags); 129 130 return 0; 131 } 132 133 static int stmmac_enable(struct ptp_clock_info *ptp, 134 struct ptp_clock_request *rq, int on) 135 { 136 struct stmmac_priv *priv = 137 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 138 struct stmmac_pps_cfg *cfg; 139 int ret = -EOPNOTSUPP; 140 unsigned long flags; 141 142 switch (rq->type) { 143 case PTP_CLK_REQ_PEROUT: 144 /* Reject requests with unsupported flags */ 145 if (rq->perout.flags) 146 return -EOPNOTSUPP; 147 148 cfg = &priv->pps[rq->perout.index]; 149 150 cfg->start.tv_sec = rq->perout.start.sec; 151 cfg->start.tv_nsec = rq->perout.start.nsec; 152 cfg->period.tv_sec = rq->perout.period.sec; 153 cfg->period.tv_nsec = rq->perout.period.nsec; 154 155 spin_lock_irqsave(&priv->ptp_lock, flags); 156 ret = stmmac_flex_pps_config(priv, priv->ioaddr, 157 rq->perout.index, cfg, on, 158 priv->sub_second_inc, 159 priv->systime_flags); 160 spin_unlock_irqrestore(&priv->ptp_lock, flags); 161 break; 162 default: 163 break; 164 } 165 166 return ret; 167 } 168 169 /** 170 * stmmac_get_syncdevicetime 171 * @device: current device time 172 * @system: system counter value read synchronously with device time 173 * @ctx: context provided by timekeeping code 174 * Description: Read device and system clock simultaneously and return the 175 * corrected clock values in ns. 176 **/ 177 static int stmmac_get_syncdevicetime(ktime_t *device, 178 struct system_counterval_t *system, 179 void *ctx) 180 { 181 struct stmmac_priv *priv = (struct stmmac_priv *)ctx; 182 183 if (priv->plat->crosststamp) 184 return priv->plat->crosststamp(device, system, ctx); 185 else 186 return -EOPNOTSUPP; 187 } 188 189 static int stmmac_getcrosststamp(struct ptp_clock_info *ptp, 190 struct system_device_crosststamp *xtstamp) 191 { 192 struct stmmac_priv *priv = 193 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 194 195 return get_device_system_crosststamp(stmmac_get_syncdevicetime, 196 priv, NULL, xtstamp); 197 } 198 199 /* structure describing a PTP hardware clock */ 200 static struct ptp_clock_info stmmac_ptp_clock_ops = { 201 .owner = THIS_MODULE, 202 .name = "stmmac ptp", 203 .max_adj = 62500000, 204 .n_alarm = 0, 205 .n_ext_ts = 0, 206 .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */ 207 .n_pins = 0, 208 .pps = 0, 209 .adjfreq = stmmac_adjust_freq, 210 .adjtime = stmmac_adjust_time, 211 .gettime64 = stmmac_get_time, 212 .settime64 = stmmac_set_time, 213 .enable = stmmac_enable, 214 .getcrosststamp = stmmac_getcrosststamp, 215 }; 216 217 /** 218 * stmmac_ptp_register 219 * @priv: driver private structure 220 * Description: this function will register the ptp clock driver 221 * to kernel. It also does some house keeping work. 222 */ 223 void stmmac_ptp_register(struct stmmac_priv *priv) 224 { 225 int i; 226 227 if (priv->plat->ptp_clk_freq_config) 228 priv->plat->ptp_clk_freq_config(priv); 229 230 for (i = 0; i < priv->dma_cap.pps_out_num; i++) { 231 if (i >= STMMAC_PPS_MAX) 232 break; 233 priv->pps[i].available = true; 234 } 235 236 if (priv->plat->ptp_max_adj) 237 stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj; 238 239 stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num; 240 241 spin_lock_init(&priv->ptp_lock); 242 priv->ptp_clock_ops = stmmac_ptp_clock_ops; 243 244 priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops, 245 priv->device); 246 if (IS_ERR(priv->ptp_clock)) { 247 netdev_err(priv->dev, "ptp_clock_register failed\n"); 248 priv->ptp_clock = NULL; 249 } else if (priv->ptp_clock) 250 netdev_info(priv->dev, "registered PTP clock\n"); 251 } 252 253 /** 254 * stmmac_ptp_unregister 255 * @priv: driver private structure 256 * Description: this function will remove/unregister the ptp clock driver 257 * from the kernel. 258 */ 259 void stmmac_ptp_unregister(struct stmmac_priv *priv) 260 { 261 if (priv->ptp_clock) { 262 ptp_clock_unregister(priv->ptp_clock); 263 priv->ptp_clock = NULL; 264 pr_debug("Removed PTP HW clock successfully on %s\n", 265 priv->dev->name); 266 } 267 } 268