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 | */ |
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 | */ |
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 | */ |
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(list: &pcdev->pse_control_head); |
79 | |
80 | mutex_lock(&pse_list_mutex); |
81 | list_add(new: &pcdev->list, head: &pse_controller_list); |
82 | mutex_unlock(lock: &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 | */ |
92 | void pse_controller_unregister(struct pse_controller_dev *pcdev) |
93 | { |
94 | mutex_lock(&pse_list_mutex); |
95 | list_del(entry: &pcdev->list); |
96 | mutex_unlock(lock: &pse_list_mutex); |
97 | } |
98 | EXPORT_SYMBOL_GPL(pse_controller_unregister); |
99 | |
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 | */ |
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(res: pcdevp); |
128 | return ret; |
129 | } |
130 | |
131 | *pcdevp = pcdev; |
132 | devres_add(dev, res: pcdevp); |
133 | |
134 | return 0; |
135 | } |
136 | EXPORT_SYMBOL_GPL(devm_pse_controller_register); |
137 | |
138 | /* PSE control section */ |
139 | |
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(module: psec->pcdev->owner); |
148 | |
149 | list_del(entry: &psec->list); |
150 | kfree(objp: psec); |
151 | } |
152 | |
153 | static void __pse_control_put_internal(struct pse_control *psec) |
154 | { |
155 | lockdep_assert_held(&pse_list_mutex); |
156 | |
157 | kref_put(kref: &psec->refcnt, release: __pse_control_release); |
158 | } |
159 | |
160 | /** |
161 | * pse_control_put - free the PSE control |
162 | * @psec: PSE control pointer |
163 | */ |
164 | void pse_control_put(struct pse_control *psec) |
165 | { |
166 | if (IS_ERR_OR_NULL(ptr: psec)) |
167 | return; |
168 | |
169 | mutex_lock(&pse_list_mutex); |
170 | __pse_control_put_internal(psec); |
171 | mutex_unlock(lock: &pse_list_mutex); |
172 | } |
173 | EXPORT_SYMBOL_GPL(pse_control_put); |
174 | |
175 | static struct pse_control * |
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(kref: &psec->refcnt); |
185 | return psec; |
186 | } |
187 | } |
188 | |
189 | psec = kzalloc(size: sizeof(*psec), GFP_KERNEL); |
190 | if (!psec) |
191 | return ERR_PTR(error: -ENOMEM); |
192 | |
193 | if (!try_module_get(module: pcdev->owner)) { |
194 | kfree(objp: psec); |
195 | return ERR_PTR(error: -ENODEV); |
196 | } |
197 | |
198 | psec->pcdev = pcdev; |
199 | list_add(new: &psec->list, head: &pcdev->pse_control_head); |
200 | psec->id = index; |
201 | kref_init(kref: &psec->refcnt); |
202 | |
203 | return psec; |
204 | } |
205 | |
206 | struct pse_control * |
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(error: -EINVAL); |
217 | |
218 | ret = of_parse_phandle_with_args(np: node, list_name: "pses" , cells_name: "#pse-cells" , index: 0, out_args: &args); |
219 | if (ret) |
220 | return ERR_PTR(error: 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(error: -EPROBE_DEFER); |
233 | goto out; |
234 | } |
235 | |
236 | if (WARN_ON(args.args_count != pcdev->of_pse_n_cells)) { |
237 | psec = ERR_PTR(error: -EINVAL); |
238 | goto out; |
239 | } |
240 | |
241 | psec_id = pcdev->of_xlate(pcdev, &args); |
242 | if (psec_id < 0) { |
243 | psec = ERR_PTR(error: 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, index: psec_id); |
249 | |
250 | out: |
251 | mutex_unlock(lock: &pse_list_mutex); |
252 | of_node_put(node: 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 | */ |
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(lock: &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 | */ |
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(lock: &psec->pcdev->lock); |
311 | |
312 | return err; |
313 | } |
314 | EXPORT_SYMBOL_GPL(pse_ethtool_set_config); |
315 | |