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