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 */
stmmac_adjust_freq(struct ptp_clock_info * ptp,s32 ppb)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 */
stmmac_adjust_time(struct ptp_clock_info * ptp,s64 delta)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 */
stmmac_get_time(struct ptp_clock_info * ptp,struct timespec64 * ts)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 */
stmmac_set_time(struct ptp_clock_info * ptp,const struct timespec64 * ts)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
stmmac_enable(struct ptp_clock_info * ptp,struct ptp_clock_request * rq,int on)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",
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 */
stmmac_ptp_register(struct stmmac_priv * priv)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 */
stmmac_ptp_unregister(struct stmmac_priv * priv)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