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
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
62
63
64
65
66
67
68static 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
98
99
100
101
102
103
104
105static 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
125
126
127
128
129
130
131
132static 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
148static int stmmac_enable(struct ptp_clock_info *ptp,
149 struct ptp_clock_request *rq, int on)
150{
151 return -EOPNOTSUPP;
152}
153
154
155static 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
173
174
175
176
177void 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
193
194
195
196
197void 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