1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20#include "stmmac.h"
21#include "stmmac_ptp.h"
22
23
24
25
26
27
28
29
30
31static 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
60
61
62
63
64
65
66static 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
94
95
96
97
98
99
100
101static 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
119
120
121
122
123
124
125
126static 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
140static 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
152 if (rq->perout.flags)
153 return -EOPNOTSUPP;
154
155 cfg = &priv->pps[rq->perout.index];
156
157 cfg->start.tv_sec = rq->perout.start.sec;
158 cfg->start.tv_nsec = rq->perout.start.nsec;
159 cfg->period.tv_sec = rq->perout.period.sec;
160 cfg->period.tv_nsec = rq->perout.period.nsec;
161
162 spin_lock_irqsave(&priv->ptp_lock, flags);
163 ret = stmmac_flex_pps_config(priv, priv->ioaddr,
164 rq->perout.index, cfg, on,
165 priv->sub_second_inc,
166 priv->systime_flags);
167 spin_unlock_irqrestore(&priv->ptp_lock, flags);
168 break;
169 default:
170 break;
171 }
172
173 return ret;
174}
175
176
177static struct ptp_clock_info stmmac_ptp_clock_ops = {
178 .owner = THIS_MODULE,
179 .name = "stmmac_ptp_clock",
180 .max_adj = 62500000,
181 .n_alarm = 0,
182 .n_ext_ts = 0,
183 .n_per_out = 0,
184 .n_pins = 0,
185 .pps = 0,
186 .adjfreq = stmmac_adjust_freq,
187 .adjtime = stmmac_adjust_time,
188 .gettime64 = stmmac_get_time,
189 .settime64 = stmmac_set_time,
190 .enable = stmmac_enable,
191};
192
193
194
195
196
197
198
199void stmmac_ptp_register(struct stmmac_priv *priv)
200{
201 int i;
202
203 for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
204 if (i >= STMMAC_PPS_MAX)
205 break;
206 priv->pps[i].available = true;
207 }
208
209 stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
210
211 spin_lock_init(&priv->ptp_lock);
212 priv->ptp_clock_ops = stmmac_ptp_clock_ops;
213
214 priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
215 priv->device);
216 if (IS_ERR(priv->ptp_clock)) {
217 netdev_err(priv->dev, "ptp_clock_register failed\n");
218 priv->ptp_clock = NULL;
219 } else if (priv->ptp_clock)
220 netdev_info(priv->dev, "registered PTP clock\n");
221}
222
223
224
225
226
227
228
229void stmmac_ptp_unregister(struct stmmac_priv *priv)
230{
231 if (priv->ptp_clock) {
232 ptp_clock_unregister(priv->ptp_clock);
233 priv->ptp_clock = NULL;
234 pr_debug("Removed PTP HW clock successfully on %s\n",
235 priv->dev->name);
236 }
237}
238