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