1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright 2013-2016 Freescale Semiconductor Inc.
4 * Copyright 2016-2018 NXP
5 */
6
7 #include <linux/module.h>
8 #include <linux/slab.h>
9 #include <linux/ptp_clock_kernel.h>
10 #include <linux/fsl/mc.h>
11
12 #include "rtc.h"
13
14 struct ptp_dpaa2_priv {
15 struct fsl_mc_device *rtc_mc_dev;
16 struct ptp_clock *clock;
17 struct ptp_clock_info caps;
18 u32 freq_comp;
19 };
20
21 /* PTP clock operations */
ptp_dpaa2_adjfreq(struct ptp_clock_info * ptp,s32 ppb)22 static int ptp_dpaa2_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
23 {
24 struct ptp_dpaa2_priv *ptp_dpaa2 =
25 container_of(ptp, struct ptp_dpaa2_priv, caps);
26 struct fsl_mc_device *mc_dev = ptp_dpaa2->rtc_mc_dev;
27 struct device *dev = &mc_dev->dev;
28 u64 adj;
29 u32 diff, tmr_add;
30 int neg_adj = 0;
31 int err = 0;
32
33 if (ppb < 0) {
34 neg_adj = 1;
35 ppb = -ppb;
36 }
37
38 tmr_add = ptp_dpaa2->freq_comp;
39 adj = tmr_add;
40 adj *= ppb;
41 diff = div_u64(adj, 1000000000ULL);
42
43 tmr_add = neg_adj ? tmr_add - diff : tmr_add + diff;
44
45 err = dprtc_set_freq_compensation(mc_dev->mc_io, 0,
46 mc_dev->mc_handle, tmr_add);
47 if (err)
48 dev_err(dev, "dprtc_set_freq_compensation err %d\n", err);
49 return 0;
50 }
51
ptp_dpaa2_adjtime(struct ptp_clock_info * ptp,s64 delta)52 static int ptp_dpaa2_adjtime(struct ptp_clock_info *ptp, s64 delta)
53 {
54 struct ptp_dpaa2_priv *ptp_dpaa2 =
55 container_of(ptp, struct ptp_dpaa2_priv, caps);
56 struct fsl_mc_device *mc_dev = ptp_dpaa2->rtc_mc_dev;
57 struct device *dev = &mc_dev->dev;
58 s64 now;
59 int err = 0;
60
61 err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &now);
62 if (err) {
63 dev_err(dev, "dprtc_get_time err %d\n", err);
64 return 0;
65 }
66
67 now += delta;
68
69 err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, now);
70 if (err) {
71 dev_err(dev, "dprtc_set_time err %d\n", err);
72 return 0;
73 }
74 return 0;
75 }
76
ptp_dpaa2_gettime(struct ptp_clock_info * ptp,struct timespec64 * ts)77 static int ptp_dpaa2_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
78 {
79 struct ptp_dpaa2_priv *ptp_dpaa2 =
80 container_of(ptp, struct ptp_dpaa2_priv, caps);
81 struct fsl_mc_device *mc_dev = ptp_dpaa2->rtc_mc_dev;
82 struct device *dev = &mc_dev->dev;
83 u64 ns;
84 u32 remainder;
85 int err = 0;
86
87 err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &ns);
88 if (err) {
89 dev_err(dev, "dprtc_get_time err %d\n", err);
90 return 0;
91 }
92
93 ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
94 ts->tv_nsec = remainder;
95 return 0;
96 }
97
ptp_dpaa2_settime(struct ptp_clock_info * ptp,const struct timespec64 * ts)98 static int ptp_dpaa2_settime(struct ptp_clock_info *ptp,
99 const struct timespec64 *ts)
100 {
101 struct ptp_dpaa2_priv *ptp_dpaa2 =
102 container_of(ptp, struct ptp_dpaa2_priv, caps);
103 struct fsl_mc_device *mc_dev = ptp_dpaa2->rtc_mc_dev;
104 struct device *dev = &mc_dev->dev;
105 u64 ns;
106 int err = 0;
107
108 ns = ts->tv_sec * 1000000000ULL;
109 ns += ts->tv_nsec;
110
111 err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, ns);
112 if (err)
113 dev_err(dev, "dprtc_set_time err %d\n", err);
114 return 0;
115 }
116
117 static struct ptp_clock_info ptp_dpaa2_caps = {
118 .owner = THIS_MODULE,
119 .name = "DPAA2 PTP Clock",
120 .max_adj = 512000,
121 .n_alarm = 2,
122 .n_ext_ts = 2,
123 .n_per_out = 3,
124 .n_pins = 0,
125 .pps = 1,
126 .adjfreq = ptp_dpaa2_adjfreq,
127 .adjtime = ptp_dpaa2_adjtime,
128 .gettime64 = ptp_dpaa2_gettime,
129 .settime64 = ptp_dpaa2_settime,
130 };
131
rtc_probe(struct fsl_mc_device * mc_dev)132 static int rtc_probe(struct fsl_mc_device *mc_dev)
133 {
134 struct device *dev = &mc_dev->dev;
135 struct ptp_dpaa2_priv *ptp_dpaa2;
136 u32 tmr_add = 0;
137 int err;
138
139 ptp_dpaa2 = kzalloc(sizeof(*ptp_dpaa2), GFP_KERNEL);
140 if (!ptp_dpaa2)
141 return -ENOMEM;
142
143 err = fsl_mc_portal_allocate(mc_dev, 0, &mc_dev->mc_io);
144 if (err) {
145 dev_err(dev, "fsl_mc_portal_allocate err %d\n", err);
146 goto err_exit;
147 }
148
149 err = dprtc_open(mc_dev->mc_io, 0, mc_dev->obj_desc.id,
150 &mc_dev->mc_handle);
151 if (err) {
152 dev_err(dev, "dprtc_open err %d\n", err);
153 goto err_free_mcp;
154 }
155
156 ptp_dpaa2->rtc_mc_dev = mc_dev;
157
158 err = dprtc_get_freq_compensation(mc_dev->mc_io, 0,
159 mc_dev->mc_handle, &tmr_add);
160 if (err) {
161 dev_err(dev, "dprtc_get_freq_compensation err %d\n", err);
162 goto err_close;
163 }
164
165 ptp_dpaa2->freq_comp = tmr_add;
166 ptp_dpaa2->caps = ptp_dpaa2_caps;
167
168 ptp_dpaa2->clock = ptp_clock_register(&ptp_dpaa2->caps, dev);
169 if (IS_ERR(ptp_dpaa2->clock)) {
170 err = PTR_ERR(ptp_dpaa2->clock);
171 goto err_close;
172 }
173
174 dpaa2_phc_index = ptp_clock_index(ptp_dpaa2->clock);
175
176 dev_set_drvdata(dev, ptp_dpaa2);
177
178 return 0;
179
180 err_close:
181 dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
182 err_free_mcp:
183 fsl_mc_portal_free(mc_dev->mc_io);
184 err_exit:
185 kfree(ptp_dpaa2);
186 dev_set_drvdata(dev, NULL);
187 return err;
188 }
189
rtc_remove(struct fsl_mc_device * mc_dev)190 static int rtc_remove(struct fsl_mc_device *mc_dev)
191 {
192 struct ptp_dpaa2_priv *ptp_dpaa2;
193 struct device *dev = &mc_dev->dev;
194
195 ptp_dpaa2 = dev_get_drvdata(dev);
196 ptp_clock_unregister(ptp_dpaa2->clock);
197
198 dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
199 fsl_mc_portal_free(mc_dev->mc_io);
200
201 kfree(ptp_dpaa2);
202 dev_set_drvdata(dev, NULL);
203
204 return 0;
205 }
206
207 static const struct fsl_mc_device_id rtc_match_id_table[] = {
208 {
209 .vendor = FSL_MC_VENDOR_FREESCALE,
210 .obj_type = "dprtc",
211 },
212 {}
213 };
214 MODULE_DEVICE_TABLE(fslmc, rtc_match_id_table);
215
216 static struct fsl_mc_driver rtc_drv = {
217 .driver = {
218 .name = KBUILD_MODNAME,
219 .owner = THIS_MODULE,
220 },
221 .probe = rtc_probe,
222 .remove = rtc_remove,
223 .match_id_table = rtc_match_id_table,
224 };
225
226 module_fsl_mc_driver(rtc_drv);
227
228 MODULE_LICENSE("GPL v2");
229 MODULE_DESCRIPTION("DPAA2 PTP Clock Driver");
230
