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 "dpaa2-ptp.h"
13 
14 struct ptp_dpaa2_priv {
15 	struct fsl_mc_device *ptp_mc_dev;
16 	struct ptp_clock *clock;
17 	struct ptp_clock_info caps;
18 	u32 freq_comp;
19 };
20 
21 /* PTP clock operations */
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->ptp_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 err;
50 }
51 
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->ptp_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 err;
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 err;
73 }
74 
75 static int ptp_dpaa2_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
76 {
77 	struct ptp_dpaa2_priv *ptp_dpaa2 =
78 		container_of(ptp, struct ptp_dpaa2_priv, caps);
79 	struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
80 	struct device *dev = &mc_dev->dev;
81 	u64 ns;
82 	u32 remainder;
83 	int err = 0;
84 
85 	err = dprtc_get_time(mc_dev->mc_io, 0, mc_dev->mc_handle, &ns);
86 	if (err) {
87 		dev_err(dev, "dprtc_get_time err %d\n", err);
88 		return err;
89 	}
90 
91 	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
92 	ts->tv_nsec = remainder;
93 	return err;
94 }
95 
96 static int ptp_dpaa2_settime(struct ptp_clock_info *ptp,
97 			     const struct timespec64 *ts)
98 {
99 	struct ptp_dpaa2_priv *ptp_dpaa2 =
100 		container_of(ptp, struct ptp_dpaa2_priv, caps);
101 	struct fsl_mc_device *mc_dev = ptp_dpaa2->ptp_mc_dev;
102 	struct device *dev = &mc_dev->dev;
103 	u64 ns;
104 	int err = 0;
105 
106 	ns = ts->tv_sec * 1000000000ULL;
107 	ns += ts->tv_nsec;
108 
109 	err = dprtc_set_time(mc_dev->mc_io, 0, mc_dev->mc_handle, ns);
110 	if (err)
111 		dev_err(dev, "dprtc_set_time err %d\n", err);
112 	return err;
113 }
114 
115 static const struct ptp_clock_info ptp_dpaa2_caps = {
116 	.owner		= THIS_MODULE,
117 	.name		= "DPAA2 PTP Clock",
118 	.max_adj	= 512000,
119 	.n_alarm	= 2,
120 	.n_ext_ts	= 2,
121 	.n_per_out	= 3,
122 	.n_pins		= 0,
123 	.pps		= 1,
124 	.adjfreq	= ptp_dpaa2_adjfreq,
125 	.adjtime	= ptp_dpaa2_adjtime,
126 	.gettime64	= ptp_dpaa2_gettime,
127 	.settime64	= ptp_dpaa2_settime,
128 };
129 
130 static int dpaa2_ptp_probe(struct fsl_mc_device *mc_dev)
131 {
132 	struct device *dev = &mc_dev->dev;
133 	struct ptp_dpaa2_priv *ptp_dpaa2;
134 	u32 tmr_add = 0;
135 	int err;
136 
137 	ptp_dpaa2 = devm_kzalloc(dev, sizeof(*ptp_dpaa2), GFP_KERNEL);
138 	if (!ptp_dpaa2)
139 		return -ENOMEM;
140 
141 	err = fsl_mc_portal_allocate(mc_dev, 0, &mc_dev->mc_io);
142 	if (err) {
143 		if (err == -ENXIO)
144 			err = -EPROBE_DEFER;
145 		else
146 			dev_err(dev, "fsl_mc_portal_allocate err %d\n", err);
147 		goto err_exit;
148 	}
149 
150 	err = dprtc_open(mc_dev->mc_io, 0, mc_dev->obj_desc.id,
151 			 &mc_dev->mc_handle);
152 	if (err) {
153 		dev_err(dev, "dprtc_open err %d\n", err);
154 		goto err_free_mcp;
155 	}
156 
157 	ptp_dpaa2->ptp_mc_dev = mc_dev;
158 
159 	err = dprtc_get_freq_compensation(mc_dev->mc_io, 0,
160 					  mc_dev->mc_handle, &tmr_add);
161 	if (err) {
162 		dev_err(dev, "dprtc_get_freq_compensation err %d\n", err);
163 		goto err_close;
164 	}
165 
166 	ptp_dpaa2->freq_comp = tmr_add;
167 	ptp_dpaa2->caps = ptp_dpaa2_caps;
168 
169 	ptp_dpaa2->clock = ptp_clock_register(&ptp_dpaa2->caps, dev);
170 	if (IS_ERR(ptp_dpaa2->clock)) {
171 		err = PTR_ERR(ptp_dpaa2->clock);
172 		goto err_close;
173 	}
174 
175 	dpaa2_phc_index = ptp_clock_index(ptp_dpaa2->clock);
176 
177 	dev_set_drvdata(dev, ptp_dpaa2);
178 
179 	return 0;
180 
181 err_close:
182 	dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
183 err_free_mcp:
184 	fsl_mc_portal_free(mc_dev->mc_io);
185 err_exit:
186 	return err;
187 }
188 
189 static int dpaa2_ptp_remove(struct fsl_mc_device *mc_dev)
190 {
191 	struct ptp_dpaa2_priv *ptp_dpaa2;
192 	struct device *dev = &mc_dev->dev;
193 
194 	ptp_dpaa2 = dev_get_drvdata(dev);
195 	ptp_clock_unregister(ptp_dpaa2->clock);
196 
197 	dprtc_close(mc_dev->mc_io, 0, mc_dev->mc_handle);
198 	fsl_mc_portal_free(mc_dev->mc_io);
199 
200 	return 0;
201 }
202 
203 static const struct fsl_mc_device_id dpaa2_ptp_match_id_table[] = {
204 	{
205 		.vendor = FSL_MC_VENDOR_FREESCALE,
206 		.obj_type = "dprtc",
207 	},
208 	{}
209 };
210 MODULE_DEVICE_TABLE(fslmc, dpaa2_ptp_match_id_table);
211 
212 static struct fsl_mc_driver dpaa2_ptp_drv = {
213 	.driver = {
214 		.name = KBUILD_MODNAME,
215 		.owner = THIS_MODULE,
216 	},
217 	.probe = dpaa2_ptp_probe,
218 	.remove = dpaa2_ptp_remove,
219 	.match_id_table = dpaa2_ptp_match_id_table,
220 };
221 
222 module_fsl_mc_driver(dpaa2_ptp_drv);
223 
224 MODULE_LICENSE("GPL v2");
225 MODULE_DESCRIPTION("DPAA2 PTP Clock Driver");
226