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 53 priv->hw->ptp->config_addend(priv->ptpaddr, addend); 54 55 spin_unlock_irqrestore(&priv->ptp_lock, flags); 56 57 return 0; 58 } 59 60 /** 61 * stmmac_adjust_time 62 * 63 * @ptp: pointer to ptp_clock_info structure 64 * @delta: desired change in nanoseconds 65 * 66 * Description: this function will shift/adjust the hardware clock time. 67 */ 68 static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta) 69 { 70 struct stmmac_priv *priv = 71 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 72 unsigned long flags; 73 u32 sec, nsec; 74 u32 quotient, reminder; 75 int neg_adj = 0; 76 77 if (delta < 0) { 78 neg_adj = 1; 79 delta = -delta; 80 } 81 82 quotient = div_u64_rem(delta, 1000000000ULL, &reminder); 83 sec = quotient; 84 nsec = reminder; 85 86 spin_lock_irqsave(&priv->ptp_lock, flags); 87 88 priv->hw->ptp->adjust_systime(priv->ptpaddr, sec, nsec, neg_adj, 89 priv->plat->has_gmac4); 90 91 spin_unlock_irqrestore(&priv->ptp_lock, flags); 92 93 return 0; 94 } 95 96 /** 97 * stmmac_get_time 98 * 99 * @ptp: pointer to ptp_clock_info structure 100 * @ts: pointer to hold time/result 101 * 102 * Description: this function will read the current time from the 103 * hardware clock and store it in @ts. 104 */ 105 static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts) 106 { 107 struct stmmac_priv *priv = 108 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 109 unsigned long flags; 110 u64 ns; 111 112 spin_lock_irqsave(&priv->ptp_lock, flags); 113 114 ns = priv->hw->ptp->get_systime(priv->ptpaddr); 115 116 spin_unlock_irqrestore(&priv->ptp_lock, flags); 117 118 *ts = ns_to_timespec64(ns); 119 120 return 0; 121 } 122 123 /** 124 * stmmac_set_time 125 * 126 * @ptp: pointer to ptp_clock_info structure 127 * @ts: time value to set 128 * 129 * Description: this function will set the current time on the 130 * hardware clock. 131 */ 132 static int stmmac_set_time(struct ptp_clock_info *ptp, 133 const struct timespec64 *ts) 134 { 135 struct stmmac_priv *priv = 136 container_of(ptp, struct stmmac_priv, ptp_clock_ops); 137 unsigned long flags; 138 139 spin_lock_irqsave(&priv->ptp_lock, flags); 140 141 priv->hw->ptp->init_systime(priv->ptpaddr, ts->tv_sec, ts->tv_nsec); 142 143 spin_unlock_irqrestore(&priv->ptp_lock, flags); 144 145 return 0; 146 } 147 148 static int stmmac_enable(struct ptp_clock_info *ptp, 149 struct ptp_clock_request *rq, int on) 150 { 151 return -EOPNOTSUPP; 152 } 153 154 /* structure describing a PTP hardware clock */ 155 static const struct ptp_clock_info stmmac_ptp_clock_ops = { 156 .owner = THIS_MODULE, 157 .name = "stmmac_ptp_clock", 158 .max_adj = 62500000, 159 .n_alarm = 0, 160 .n_ext_ts = 0, 161 .n_per_out = 0, 162 .n_pins = 0, 163 .pps = 0, 164 .adjfreq = stmmac_adjust_freq, 165 .adjtime = stmmac_adjust_time, 166 .gettime64 = stmmac_get_time, 167 .settime64 = stmmac_set_time, 168 .enable = stmmac_enable, 169 }; 170 171 /** 172 * stmmac_ptp_register 173 * @priv: driver private structure 174 * Description: this function will register the ptp clock driver 175 * to kernel. It also does some house keeping work. 176 */ 177 void stmmac_ptp_register(struct stmmac_priv *priv) 178 { 179 spin_lock_init(&priv->ptp_lock); 180 priv->ptp_clock_ops = stmmac_ptp_clock_ops; 181 182 priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops, 183 priv->device); 184 if (IS_ERR(priv->ptp_clock)) { 185 netdev_err(priv->dev, "ptp_clock_register failed\n"); 186 priv->ptp_clock = NULL; 187 } else if (priv->ptp_clock) 188 netdev_info(priv->dev, "registered PTP clock\n"); 189 } 190 191 /** 192 * stmmac_ptp_unregister 193 * @priv: driver private structure 194 * Description: this function will remove/unregister the ptp clock driver 195 * from the kernel. 196 */ 197 void stmmac_ptp_unregister(struct stmmac_priv *priv) 198 { 199 if (priv->ptp_clock) { 200 ptp_clock_unregister(priv->ptp_clock); 201 priv->ptp_clock = NULL; 202 pr_debug("Removed PTP HW clock successfully on %s\n", 203 priv->dev->name); 204 } 205 } 206