1
2
3
4
5
6
7
8
9
10#include "stmmac.h"
11#include "stmmac_ptp.h"
12
13
14
15
16
17
18
19
20
21static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
22{
23 struct stmmac_priv *priv =
24 container_of(ptp, struct stmmac_priv, ptp_clock_ops);
25 unsigned long flags;
26 u32 diff, addend;
27 int neg_adj = 0;
28 u64 adj;
29
30 if (ppb < 0) {
31 neg_adj = 1;
32 ppb = -ppb;
33 }
34
35 addend = priv->default_addend;
36 adj = addend;
37 adj *= ppb;
38 diff = div_u64(adj, 1000000000ULL);
39 addend = neg_adj ? (addend - diff) : (addend + diff);
40
41 spin_lock_irqsave(&priv->ptp_lock, flags);
42 stmmac_config_addend(priv, priv->ptpaddr, addend);
43 spin_unlock_irqrestore(&priv->ptp_lock, flags);
44
45 return 0;
46}
47
48
49
50
51
52
53
54
55
56static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
57{
58 struct stmmac_priv *priv =
59 container_of(ptp, struct stmmac_priv, ptp_clock_ops);
60 unsigned long flags;
61 u32 sec, nsec;
62 u32 quotient, reminder;
63 int neg_adj = 0;
64 bool xmac;
65
66 xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;
67
68 if (delta < 0) {
69 neg_adj = 1;
70 delta = -delta;
71 }
72
73 quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
74 sec = quotient;
75 nsec = reminder;
76
77 spin_lock_irqsave(&priv->ptp_lock, flags);
78 stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
79 spin_unlock_irqrestore(&priv->ptp_lock, flags);
80
81 return 0;
82}
83
84
85
86
87
88
89
90
91
92
93static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
94{
95 struct stmmac_priv *priv =
96 container_of(ptp, struct stmmac_priv, ptp_clock_ops);
97 unsigned long flags;
98 u64 ns = 0;
99
100 spin_lock_irqsave(&priv->ptp_lock, flags);
101 stmmac_get_systime(priv, priv->ptpaddr, &ns);
102 spin_unlock_irqrestore(&priv->ptp_lock, flags);
103
104 *ts = ns_to_timespec64(ns);
105
106 return 0;
107}
108
109
110
111
112
113
114
115
116
117
118static int stmmac_set_time(struct ptp_clock_info *ptp,
119 const struct timespec64 *ts)
120{
121 struct stmmac_priv *priv =
122 container_of(ptp, struct stmmac_priv, ptp_clock_ops);
123 unsigned long flags;
124
125 spin_lock_irqsave(&priv->ptp_lock, flags);
126 stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
127 spin_unlock_irqrestore(&priv->ptp_lock, flags);
128
129 return 0;
130}
131
132static int stmmac_enable(struct ptp_clock_info *ptp,
133 struct ptp_clock_request *rq, int on)
134{
135 struct stmmac_priv *priv =
136 container_of(ptp, struct stmmac_priv, ptp_clock_ops);
137 struct stmmac_pps_cfg *cfg;
138 int ret = -EOPNOTSUPP;
139 unsigned long flags;
140
141 switch (rq->type) {
142 case PTP_CLK_REQ_PEROUT:
143 cfg = &priv->pps[rq->perout.index];
144
145 cfg->start.tv_sec = rq->perout.start.sec;
146 cfg->start.tv_nsec = rq->perout.start.nsec;
147 cfg->period.tv_sec = rq->perout.period.sec;
148 cfg->period.tv_nsec = rq->perout.period.nsec;
149
150 spin_lock_irqsave(&priv->ptp_lock, flags);
151 ret = stmmac_flex_pps_config(priv, priv->ioaddr,
152 rq->perout.index, cfg, on,
153 priv->sub_second_inc,
154 priv->systime_flags);
155 spin_unlock_irqrestore(&priv->ptp_lock, flags);
156 break;
157 default:
158 break;
159 }
160
161 return ret;
162}
163
164
165static struct ptp_clock_info stmmac_ptp_clock_ops = {
166 .owner = THIS_MODULE,
167 .name = "stmmac_ptp_clock",
168 .max_adj = 62500000,
169 .n_alarm = 0,
170 .n_ext_ts = 0,
171 .n_per_out = 0,
172 .n_pins = 0,
173 .pps = 0,
174 .adjfreq = stmmac_adjust_freq,
175 .adjtime = stmmac_adjust_time,
176 .gettime64 = stmmac_get_time,
177 .settime64 = stmmac_set_time,
178 .enable = stmmac_enable,
179};
180
181
182
183
184
185
186
187void stmmac_ptp_register(struct stmmac_priv *priv)
188{
189 int i;
190
191 for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
192 if (i >= STMMAC_PPS_MAX)
193 break;
194 priv->pps[i].available = true;
195 }
196
197 stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
198
199 spin_lock_init(&priv->ptp_lock);
200 priv->ptp_clock_ops = stmmac_ptp_clock_ops;
201
202 priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
203 priv->device);
204 if (IS_ERR(priv->ptp_clock)) {
205 netdev_err(priv->dev, "ptp_clock_register failed\n");
206 priv->ptp_clock = NULL;
207 } else if (priv->ptp_clock)
208 netdev_info(priv->dev, "registered PTP clock\n");
209}
210
211
212
213
214
215
216
217void stmmac_ptp_unregister(struct stmmac_priv *priv)
218{
219 if (priv->ptp_clock) {
220 ptp_clock_unregister(priv->ptp_clock);
221 priv->ptp_clock = NULL;
222 pr_debug("Removed PTP HW clock successfully on %s\n",
223 priv->dev->name);
224 }
225}
226