]>
Commit | Line | Data |
---|---|---|
92ba6888 RK |
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 | You should have received a copy of the GNU General Public License along with | |
16 | this program; if not, write to the Free Software Foundation, Inc., | |
17 | 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA. | |
18 | ||
19 | The full GNU General Public License is included in this distribution in | |
20 | the file called "COPYING". | |
21 | ||
22 | Author: Rayagond Kokatanur <rayagond@vayavyalabs.com> | |
23 | *******************************************************************************/ | |
24 | #include "stmmac.h" | |
25 | #include "stmmac_ptp.h" | |
26 | ||
27 | /** | |
28 | * stmmac_adjust_freq | |
29 | * | |
30 | * @ptp: pointer to ptp_clock_info structure | |
31 | * @ppb: desired period change in parts ber billion | |
32 | * | |
33 | * Description: this function will adjust the frequency of hardware clock. | |
34 | */ | |
35 | static 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 | ||
7cd01399 | 59 | spin_unlock_irqrestore(&priv->ptp_lock, flags); |
92ba6888 RK |
60 | |
61 | return 0; | |
62 | } | |
63 | ||
64 | /** | |
65 | * stmmac_adjust_time | |
66 | * | |
67 | * @ptp: pointer to ptp_clock_info structure | |
68 | * @delta: desired change in nanoseconds | |
69 | * | |
70 | * Description: this function will shift/adjust the hardware clock time. | |
71 | */ | |
72 | static 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 | ||
7cd01399 | 94 | spin_unlock_irqrestore(&priv->ptp_lock, flags); |
92ba6888 RK |
95 | |
96 | return 0; | |
97 | } | |
98 | ||
99 | /** | |
100 | * stmmac_get_time | |
101 | * | |
102 | * @ptp: pointer to ptp_clock_info structure | |
103 | * @ts: pointer to hold time/result | |
104 | * | |
105 | * Description: this function will read the current time from the | |
106 | * hardware clock and store it in @ts. | |
107 | */ | |
108 | static 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 | * stmmac_set_time | |
130 | * | |
131 | * @ptp: pointer to ptp_clock_info structure | |
132 | * @ts: time value to set | |
133 | * | |
134 | * Description: this function will set the current time on the | |
135 | * hardware clock. | |
136 | */ | |
137 | static 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 | ||
153 | static int stmmac_enable(struct ptp_clock_info *ptp, | |
154 | struct ptp_clock_request *rq, int on) | |
155 | { | |
156 | return -EOPNOTSUPP; | |
157 | } | |
158 | ||
159 | /* structure describing a PTP hardware clock */ | |
160 | static 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, | |
4986b4f0 | 167 | .n_pins = 0, |
92ba6888 RK |
168 | .pps = 0, |
169 | .adjfreq = stmmac_adjust_freq, | |
170 | .adjtime = stmmac_adjust_time, | |
171 | .gettime = stmmac_get_time, | |
172 | .settime = stmmac_set_time, | |
173 | .enable = stmmac_enable, | |
174 | }; | |
175 | ||
176 | /** | |
177 | * stmmac_ptp_register | |
32ceabca | 178 | * @priv: driver private structure |
92ba6888 RK |
179 | * Description: this function will register the ptp clock driver |
180 | * to kernel. It also does some house keeping work. | |
181 | */ | |
182 | int stmmac_ptp_register(struct stmmac_priv *priv) | |
183 | { | |
184 | spin_lock_init(&priv->ptp_lock); | |
185 | priv->ptp_clock_ops = stmmac_ptp_clock_ops; | |
186 | ||
187 | priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops, | |
188 | priv->device); | |
189 | if (IS_ERR(priv->ptp_clock)) { | |
190 | priv->ptp_clock = NULL; | |
191 | pr_err("ptp_clock_register() failed on %s\n", priv->dev->name); | |
192 | } else | |
193 | pr_debug("Added PTP HW clock successfully on %s\n", | |
194 | priv->dev->name); | |
195 | ||
196 | return 0; | |
197 | } | |
198 | ||
199 | /** | |
200 | * stmmac_ptp_unregister | |
32ceabca | 201 | * @priv: driver private structure |
92ba6888 RK |
202 | * Description: this function will remove/unregister the ptp clock driver |
203 | * from the kernel. | |
204 | */ | |
205 | void stmmac_ptp_unregister(struct stmmac_priv *priv) | |
206 | { | |
207 | if (priv->ptp_clock) { | |
208 | ptp_clock_unregister(priv->ptp_clock); | |
209 | pr_debug("Removed PTP HW clock successfully on %s\n", | |
210 | priv->dev->name); | |
211 | } | |
212 | } |