1 // SPDX-License-Identifier: GPL-2.0-only
2 //
3 // Framework for Ethernet Power Sourcing Equipment
4 //
5 // Copyright (c) 2022 Pengutronix, Oleksij Rempel <kernel@pengutronix.de>
6 //
7 
8 #include <linux/device.h>
9 #include <linux/of.h>
10 #include <linux/pse-pd/pse.h>
11 
12 static DEFINE_MUTEX(pse_list_mutex);
13 static LIST_HEAD(pse_controller_list);
14 
15 /**
16  * struct pse_control - a PSE control
17  * @pcdev: a pointer to the PSE controller device
18  *         this PSE control belongs to
19  * @list: list entry for the pcdev's PSE controller list
20  * @id: ID of the PSE line in the PSE controller device
21  * @refcnt: Number of gets of this pse_control
22  */
23 struct pse_control {
24 	struct pse_controller_dev *pcdev;
25 	struct list_head list;
26 	unsigned int id;
27 	struct kref refcnt;
28 };
29 
30 /**
31  * of_pse_zero_xlate - dummy function for controllers with one only control
32  * @pcdev: a pointer to the PSE controller device
33  * @pse_spec: PSE line specifier as found in the device tree
34  *
35  * This static translation function is used by default if of_xlate in
36  * :c:type:`pse_controller_dev` is not set. It is useful for all PSE
37  * controllers with #pse-cells = <0>.
38  */
of_pse_zero_xlate(struct pse_controller_dev * pcdev,const struct of_phandle_args * pse_spec)39 static int of_pse_zero_xlate(struct pse_controller_dev *pcdev,
40 			     const struct of_phandle_args *pse_spec)
41 {
42 	return 0;
43 }
44 
45 /**
46  * of_pse_simple_xlate - translate pse_spec to the PSE line number
47  * @pcdev: a pointer to the PSE controller device
48  * @pse_spec: PSE line specifier as found in the device tree
49  *
50  * This static translation function is used by default if of_xlate in
51  * :c:type:`pse_controller_dev` is not set. It is useful for all PSE
52  * controllers with 1:1 mapping, where PSE lines can be indexed by number
53  * without gaps.
54  */
of_pse_simple_xlate(struct pse_controller_dev * pcdev,const struct of_phandle_args * pse_spec)55 static int of_pse_simple_xlate(struct pse_controller_dev *pcdev,
56 			       const struct of_phandle_args *pse_spec)
57 {
58 	if (pse_spec->args[0] >= pcdev->nr_lines)
59 		return -EINVAL;
60 
61 	return pse_spec->args[0];
62 }
63 
64 /**
65  * pse_controller_register - register a PSE controller device
66  * @pcdev: a pointer to the initialized PSE controller device
67  */
pse_controller_register(struct pse_controller_dev * pcdev)68 int pse_controller_register(struct pse_controller_dev *pcdev)
69 {
70 	if (!pcdev->of_xlate) {
71 		if (pcdev->of_pse_n_cells == 0)
72 			pcdev->of_xlate = of_pse_zero_xlate;
73 		else if (pcdev->of_pse_n_cells == 1)
74 			pcdev->of_xlate = of_pse_simple_xlate;
75 	}
76 
77 	mutex_init(&pcdev->lock);
78 	INIT_LIST_HEAD(&pcdev->pse_control_head);
79 
80 	mutex_lock(&pse_list_mutex);
81 	list_add(&pcdev->list, &pse_controller_list);
82 	mutex_unlock(&pse_list_mutex);
83 
84 	return 0;
85 }
86 EXPORT_SYMBOL_GPL(pse_controller_register);
87 
88 /**
89  * pse_controller_unregister - unregister a PSE controller device
90  * @pcdev: a pointer to the PSE controller device
91  */
pse_controller_unregister(struct pse_controller_dev * pcdev)92 void pse_controller_unregister(struct pse_controller_dev *pcdev)
93 {
94 	mutex_lock(&pse_list_mutex);
95 	list_del(&pcdev->list);
96 	mutex_unlock(&pse_list_mutex);
97 }
98 EXPORT_SYMBOL_GPL(pse_controller_unregister);
99 
devm_pse_controller_release(struct device * dev,void * res)100 static void devm_pse_controller_release(struct device *dev, void *res)
101 {
102 	pse_controller_unregister(*(struct pse_controller_dev **)res);
103 }
104 
105 /**
106  * devm_pse_controller_register - resource managed pse_controller_register()
107  * @dev: device that is registering this PSE controller
108  * @pcdev: a pointer to the initialized PSE controller device
109  *
110  * Managed pse_controller_register(). For PSE controllers registered by
111  * this function, pse_controller_unregister() is automatically called on
112  * driver detach. See pse_controller_register() for more information.
113  */
devm_pse_controller_register(struct device * dev,struct pse_controller_dev * pcdev)114 int devm_pse_controller_register(struct device *dev,
115 				 struct pse_controller_dev *pcdev)
116 {
117 	struct pse_controller_dev **pcdevp;
118 	int ret;
119 
120 	pcdevp = devres_alloc(devm_pse_controller_release, sizeof(*pcdevp),
121 			      GFP_KERNEL);
122 	if (!pcdevp)
123 		return -ENOMEM;
124 
125 	ret = pse_controller_register(pcdev);
126 	if (ret) {
127 		devres_free(pcdevp);
128 		return ret;
129 	}
130 
131 	*pcdevp = pcdev;
132 	devres_add(dev, pcdevp);
133 
134 	return 0;
135 }
136 EXPORT_SYMBOL_GPL(devm_pse_controller_register);
137 
138 /* PSE control section */
139 
__pse_control_release(struct kref * kref)140 static void __pse_control_release(struct kref *kref)
141 {
142 	struct pse_control *psec = container_of(kref, struct pse_control,
143 						  refcnt);
144 
145 	lockdep_assert_held(&pse_list_mutex);
146 
147 	module_put(psec->pcdev->owner);
148 
149 	list_del(&psec->list);
150 	kfree(psec);
151 }
152 
__pse_control_put_internal(struct pse_control * psec)153 static void __pse_control_put_internal(struct pse_control *psec)
154 {
155 	lockdep_assert_held(&pse_list_mutex);
156 
157 	kref_put(&psec->refcnt, __pse_control_release);
158 }
159 
160 /**
161  * pse_control_put - free the PSE control
162  * @psec: PSE control pointer
163  */
pse_control_put(struct pse_control * psec)164 void pse_control_put(struct pse_control *psec)
165 {
166 	if (IS_ERR_OR_NULL(psec))
167 		return;
168 
169 	mutex_lock(&pse_list_mutex);
170 	__pse_control_put_internal(psec);
171 	mutex_unlock(&pse_list_mutex);
172 }
173 EXPORT_SYMBOL_GPL(pse_control_put);
174 
175 static struct pse_control *
pse_control_get_internal(struct pse_controller_dev * pcdev,unsigned int index)176 pse_control_get_internal(struct pse_controller_dev *pcdev, unsigned int index)
177 {
178 	struct pse_control *psec;
179 
180 	lockdep_assert_held(&pse_list_mutex);
181 
182 	list_for_each_entry(psec, &pcdev->pse_control_head, list) {
183 		if (psec->id == index) {
184 			kref_get(&psec->refcnt);
185 			return psec;
186 		}
187 	}
188 
189 	psec = kzalloc(sizeof(*psec), GFP_KERNEL);
190 	if (!psec)
191 		return ERR_PTR(-ENOMEM);
192 
193 	if (!try_module_get(pcdev->owner)) {
194 		kfree(psec);
195 		return ERR_PTR(-ENODEV);
196 	}
197 
198 	psec->pcdev = pcdev;
199 	list_add(&psec->list, &pcdev->pse_control_head);
200 	psec->id = index;
201 	kref_init(&psec->refcnt);
202 
203 	return psec;
204 }
205 
206 struct pse_control *
of_pse_control_get(struct device_node * node)207 of_pse_control_get(struct device_node *node)
208 {
209 	struct pse_controller_dev *r, *pcdev;
210 	struct of_phandle_args args;
211 	struct pse_control *psec;
212 	int psec_id;
213 	int ret;
214 
215 	if (!node)
216 		return ERR_PTR(-EINVAL);
217 
218 	ret = of_parse_phandle_with_args(node, "pses", "#pse-cells", 0, &args);
219 	if (ret)
220 		return ERR_PTR(ret);
221 
222 	mutex_lock(&pse_list_mutex);
223 	pcdev = NULL;
224 	list_for_each_entry(r, &pse_controller_list, list) {
225 		if (args.np == r->dev->of_node) {
226 			pcdev = r;
227 			break;
228 		}
229 	}
230 
231 	if (!pcdev) {
232 		psec = ERR_PTR(-EPROBE_DEFER);
233 		goto out;
234 	}
235 
236 	if (WARN_ON(args.args_count != pcdev->of_pse_n_cells)) {
237 		psec = ERR_PTR(-EINVAL);
238 		goto out;
239 	}
240 
241 	psec_id = pcdev->of_xlate(pcdev, &args);
242 	if (psec_id < 0) {
243 		psec = ERR_PTR(psec_id);
244 		goto out;
245 	}
246 
247 	/* pse_list_mutex also protects the pcdev's pse_control list */
248 	psec = pse_control_get_internal(pcdev, psec_id);
249 
250 out:
251 	mutex_unlock(&pse_list_mutex);
252 	of_node_put(args.np);
253 
254 	return psec;
255 }
256 EXPORT_SYMBOL_GPL(of_pse_control_get);
257 
258 /**
259  * pse_ethtool_get_status - get status of PSE control
260  * @psec: PSE control pointer
261  * @extack: extack for reporting useful error messages
262  * @status: struct to store PSE status
263  */
pse_ethtool_get_status(struct pse_control * psec,struct netlink_ext_ack * extack,struct pse_control_status * status)264 int pse_ethtool_get_status(struct pse_control *psec,
265 			   struct netlink_ext_ack *extack,
266 			   struct pse_control_status *status)
267 {
268 	const struct pse_controller_ops *ops;
269 	int err;
270 
271 	ops = psec->pcdev->ops;
272 
273 	if (!ops->ethtool_get_status) {
274 		NL_SET_ERR_MSG(extack,
275 			       "PSE driver does not support status report");
276 		return -EOPNOTSUPP;
277 	}
278 
279 	mutex_lock(&psec->pcdev->lock);
280 	err = ops->ethtool_get_status(psec->pcdev, psec->id, extack, status);
281 	mutex_unlock(&psec->pcdev->lock);
282 
283 	return err;
284 }
285 EXPORT_SYMBOL_GPL(pse_ethtool_get_status);
286 
287 /**
288  * pse_ethtool_set_config - set PSE control configuration
289  * @psec: PSE control pointer
290  * @extack: extack for reporting useful error messages
291  * @config: Configuration of the test to run
292  */
pse_ethtool_set_config(struct pse_control * psec,struct netlink_ext_ack * extack,const struct pse_control_config * config)293 int pse_ethtool_set_config(struct pse_control *psec,
294 			   struct netlink_ext_ack *extack,
295 			   const struct pse_control_config *config)
296 {
297 	const struct pse_controller_ops *ops;
298 	int err;
299 
300 	ops = psec->pcdev->ops;
301 
302 	if (!ops->ethtool_set_config) {
303 		NL_SET_ERR_MSG(extack,
304 			       "PSE driver does not configuration");
305 		return -EOPNOTSUPP;
306 	}
307 
308 	mutex_lock(&psec->pcdev->lock);
309 	err = ops->ethtool_set_config(psec->pcdev, psec->id, extack, config);
310 	mutex_unlock(&psec->pcdev->lock);
311 
312 	return err;
313 }
314 EXPORT_SYMBOL_GPL(pse_ethtool_set_config);
315