projects/linux·blob master · refs

1983 lines · 47.6 KiB

1// SPDX-License-Identifier: GPL-2.0-only
2//
3// Framework for Ethernet Power Sourcing Equipment
4//
5// Copyright (c) 2022 Pengutronix, Oleksij Rempel <[email protected]>
6//
7
8#include <linux/device.h>
9#include <linux/ethtool.h>
10#include <linux/ethtool_netlink.h>
11#include <linux/of.h>
12#include <linux/phy.h>
13#include <linux/pse-pd/pse.h>
14#include <linux/regulator/driver.h>
15#include <linux/regulator/machine.h>
16#include <linux/rtnetlink.h>
17#include <net/net_trackers.h>
18
19#define PSE_PW_D_LIMIT INT_MAX
20
21static DEFINE_MUTEX(pse_list_mutex);
22static LIST_HEAD(pse_controller_list);
23static DEFINE_XARRAY_ALLOC(pse_pw_d_map);
24static DEFINE_MUTEX(pse_pw_d_mutex);
25
26/**
27 * struct pse_control - a PSE control
28 * @pcdev: a pointer to the PSE controller device
29 * this PSE control belongs to
30 * @ps: PSE PI supply of the PSE control
31 * @list: list entry for the pcdev's PSE controller list
32 * @id: ID of the PSE line in the PSE controller device
33 * @refcnt: Number of gets of this pse_control
34 * @attached_phydev: PHY device pointer attached by the PSE control
35 */
36struct pse_control {
37 struct pse_controller_dev *pcdev;
38 struct regulator *ps;
39 struct list_head list;
40 unsigned int id;
41 struct kref refcnt;
42 struct phy_device *attached_phydev;
43};
44
45/**
46 * struct pse_power_domain - a PSE power domain
47 * @id: ID of the power domain
48 * @supply: Power supply the Power Domain
49 * @refcnt: Number of gets of this pse_power_domain
50 * @budget_eval_strategy: Current power budget evaluation strategy of the
51 * power domain
52 */
53struct pse_power_domain {
54 int id;
55 struct regulator *supply;
56 struct kref refcnt;
57 u32 budget_eval_strategy;
58};
59
60static int of_load_single_pse_pi_pairset(struct device_node *node,
61 struct pse_pi *pi,
62 int pairset_num)
63{
64 struct device_node *pairset_np;
65 const char *name;
66 int ret;
67
68 ret = of_property_read_string_index(node, "pairset-names",
69 pairset_num, &name);
70 if (ret)
71 return ret;
72
73 if (!strcmp(name, "alternative-a")) {
74 pi->pairset[pairset_num].pinout = ALTERNATIVE_A;
75 } else if (!strcmp(name, "alternative-b")) {
76 pi->pairset[pairset_num].pinout = ALTERNATIVE_B;
77 } else {
78 pr_err("pse: wrong pairset-names value %s (%pOF)\n",
79 name, node);
80 return -EINVAL;
81 }
82
83 pairset_np = of_parse_phandle(node, "pairsets", pairset_num);
84 if (!pairset_np)
85 return -ENODEV;
86
87 pi->pairset[pairset_num].np = pairset_np;
88
89 return 0;
90}
91
92/**
93 * of_load_pse_pi_pairsets - load PSE PI pairsets pinout and polarity
94 * @node: a pointer of the device node
95 * @pi: a pointer of the PSE PI to fill
96 * @npairsets: the number of pairsets (1 or 2) used by the PI
97 *
98 * Return: 0 on success and failure value on error
99 */
100static int of_load_pse_pi_pairsets(struct device_node *node,
101 struct pse_pi *pi,
102 int npairsets)
103{
104 int i, ret;
105
106 ret = of_property_count_strings(node, "pairset-names");
107 if (ret != npairsets) {
108 pr_err("pse: amount of pairsets and pairset-names is not equal %d != %d (%pOF)\n",
109 npairsets, ret, node);
110 return -EINVAL;
111 }
112
113 for (i = 0; i < npairsets; i++) {
114 ret = of_load_single_pse_pi_pairset(node, pi, i);
115 if (ret)
116 goto out;
117 }
118
119 if (npairsets == 2 &&
120 pi->pairset[0].pinout == pi->pairset[1].pinout) {
121 pr_err("pse: two PI pairsets can not have identical pinout (%pOF)",
122 node);
123 ret = -EINVAL;
124 }
125
126out:
127 /* If an error appears, release all the pairset device node kref */
128 if (ret) {
129 of_node_put(pi->pairset[0].np);
130 pi->pairset[0].np = NULL;
131 of_node_put(pi->pairset[1].np);
132 pi->pairset[1].np = NULL;
133 }
134
135 return ret;
136}
137
138static void pse_release_pis(struct pse_controller_dev *pcdev)
139{
140 int i;
141
142 for (i = 0; i < pcdev->nr_lines; i++) {
143 of_node_put(pcdev->pi[i].pairset[0].np);
144 of_node_put(pcdev->pi[i].pairset[1].np);
145 of_node_put(pcdev->pi[i].np);
146 }
147 kfree(pcdev->pi);
148}
149
150/**
151 * of_load_pse_pis - load all the PSE PIs
152 * @pcdev: a pointer to the PSE controller device
153 *
154 * Return: 0 on success and failure value on error
155 */
156static int of_load_pse_pis(struct pse_controller_dev *pcdev)
157{
158 struct device_node *np = pcdev->dev->of_node;
159 struct device_node *node, *pis;
160 int ret;
161
162 if (!np)
163 return -ENODEV;
164
165 pcdev->pi = kzalloc_objs(*pcdev->pi, pcdev->nr_lines);
166 if (!pcdev->pi)
167 return -ENOMEM;
168
169 pis = of_get_child_by_name(np, "pse-pis");
170 if (!pis) {
171 /* no description of PSE PIs */
172 pcdev->no_of_pse_pi = true;
173 return 0;
174 }
175
176 for_each_child_of_node(pis, node) {
177 struct pse_pi pi = {0};
178 u32 id;
179
180 if (!of_node_name_eq(node, "pse-pi"))
181 continue;
182
183 ret = of_property_read_u32(node, "reg", &id);
184 if (ret) {
185 dev_err(pcdev->dev,
186 "can't get reg property for node '%pOF'",
187 node);
188 goto out;
189 }
190
191 if (id >= pcdev->nr_lines) {
192 dev_err(pcdev->dev,
193 "reg value (%u) is out of range (%u) (%pOF)\n",
194 id, pcdev->nr_lines, node);
195 ret = -EINVAL;
196 goto out;
197 }
198
199 if (pcdev->pi[id].np) {
200 dev_err(pcdev->dev,
201 "other node with same reg value was already registered. %pOF : %pOF\n",
202 pcdev->pi[id].np, node);
203 ret = -EINVAL;
204 goto out;
205 }
206
207 ret = of_count_phandle_with_args(node, "pairsets", NULL);
208 /* npairsets is limited to value one or two */
209 if (ret == 1 || ret == 2) {
210 ret = of_load_pse_pi_pairsets(node, &pi, ret);
211 if (ret)
212 goto out;
213 } else if (ret != -ENOENT) {
214 dev_err(pcdev->dev,
215 "error: wrong number of pairsets. Should be 1 or 2, got %d (%pOF)\n",
216 ret, node);
217 ret = -EINVAL;
218 goto out;
219 }
220
221 of_node_get(node);
222 pi.np = node;
223 memcpy(&pcdev->pi[id], &pi, sizeof(pi));
224 }
225
226 of_node_put(pis);
227 return 0;
228
229out:
230 pse_release_pis(pcdev);
231 of_node_put(node);
232 of_node_put(pis);
233 return ret;
234}
235
236/**
237 * pse_control_find_by_id - Find pse_control from an id
238 * @pcdev: a pointer to the PSE
239 * @id: index of the PSE control
240 *
241 * Return: pse_control pointer or NULL. The device returned has had a
242 * reference added and the pointer is safe until the user calls
243 * pse_control_put() to indicate they have finished with it.
244 */
245static struct pse_control *
246pse_control_find_by_id(struct pse_controller_dev *pcdev, int id)
247{
248 struct pse_control *psec;
249
250 mutex_lock(&pse_list_mutex);
251 list_for_each_entry(psec, &pcdev->pse_control_head, list) {
252 if (psec->id == id) {
253 kref_get(&psec->refcnt);
254 mutex_unlock(&pse_list_mutex);
255 return psec;
256 }
257 }
258 mutex_unlock(&pse_list_mutex);
259 return NULL;
260}
261
262/**
263 * pse_control_get_netdev - Return netdev associated to a PSE control
264 * @psec: PSE control pointer
265 *
266 * Return: netdev pointer or NULL
267 */
268static struct net_device *pse_control_get_netdev(struct pse_control *psec)
269{
270 ASSERT_RTNL();
271
272 if (!psec || !psec->attached_phydev)
273 return NULL;
274
275 return psec->attached_phydev->attached_dev;
276}
277
278/**
279 * pse_pi_is_hw_enabled - Is PI enabled at the hardware level
280 * @pcdev: a pointer to the PSE controller device
281 * @id: Index of the PI
282 *
283 * Return: 1 if the PI is enabled at the hardware level, 0 if not, and
284 * a failure value on error
285 */
286static int pse_pi_is_hw_enabled(struct pse_controller_dev *pcdev, int id)
287{
288 struct pse_admin_state admin_state = {0};
289 int ret;
290
291 ret = pcdev->ops->pi_get_admin_state(pcdev, id, &admin_state);
292 if (ret < 0)
293 return ret;
294
295 /* PI is well enabled at the hardware level */
296 if (admin_state.podl_admin_state == ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED ||
297 admin_state.c33_admin_state == ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED)
298 return 1;
299
300 return 0;
301}
302
303/**
304 * pse_pi_is_admin_enable_pending - Check if PI is in admin enable pending state
305 * which mean the power is not yet being
306 * delivered
307 * @pcdev: a pointer to the PSE controller device
308 * @id: Index of the PI
309 *
310 * Detects if a PI is enabled in software with a PD detected, but the hardware
311 * admin state hasn't been applied yet.
312 *
313 * This function is used in the power delivery and retry mechanisms to determine
314 * which PIs need to have power delivery attempted again.
315 *
316 * Return: true if the PI has admin enable flag set in software but not yet
317 * reflected in the hardware admin state, false otherwise.
318 */
319static bool
320pse_pi_is_admin_enable_pending(struct pse_controller_dev *pcdev, int id)
321{
322 int ret;
323
324 /* PI not enabled or nothing is plugged */
325 if (!pcdev->pi[id].admin_state_enabled ||
326 !pcdev->pi[id].isr_pd_detected)
327 return false;
328
329 ret = pse_pi_is_hw_enabled(pcdev, id);
330 /* PSE PI is already enabled at hardware level */
331 if (ret == 1)
332 return false;
333
334 return true;
335}
336
337static int _pse_pi_delivery_power_sw_pw_ctrl(struct pse_controller_dev *pcdev,
338 int id,
339 struct netlink_ext_ack *extack);
340
341/**
342 * pse_pw_d_retry_power_delivery - Retry power delivery for pending ports in a
343 * PSE power domain
344 * @pcdev: a pointer to the PSE controller device
345 * @pw_d: a pointer to the PSE power domain
346 *
347 * Scans all ports in the specified power domain and attempts to enable power
348 * delivery to any ports that have admin enable state set but don't yet have
349 * hardware power enabled. Used when there are changes in connection status,
350 * admin state, or priority that might allow previously unpowered ports to
351 * receive power, especially in over-budget conditions.
352 */
353static void pse_pw_d_retry_power_delivery(struct pse_controller_dev *pcdev,
354 struct pse_power_domain *pw_d)
355{
356 int i, ret = 0;
357
358 for (i = 0; i < pcdev->nr_lines; i++) {
359 int prio_max = pcdev->nr_lines;
360 struct netlink_ext_ack extack;
361
362 if (pcdev->pi[i].pw_d != pw_d)
363 continue;
364
365 if (!pse_pi_is_admin_enable_pending(pcdev, i))
366 continue;
367
368 /* Do not try to enable PI with a lower prio (higher value)
369 * than one which already can't be enabled.
370 */
371 if (pcdev->pi[i].prio > prio_max)
372 continue;
373
374 ret = _pse_pi_delivery_power_sw_pw_ctrl(pcdev, i, &extack);
375 if (ret == -ERANGE)
376 prio_max = pcdev->pi[i].prio;
377 }
378}
379
380/**
381 * pse_pw_d_is_sw_pw_control - Determine if power control is software managed
382 * @pcdev: a pointer to the PSE controller device
383 * @pw_d: a pointer to the PSE power domain
384 *
385 * This function determines whether the power control for a specific power
386 * domain is managed by software in the interrupt handler rather than directly
387 * by hardware.
388 *
389 * Software power control is active in the following cases:
390 * - When the budget evaluation strategy is set to static
391 * - When the budget evaluation strategy is disabled but the PSE controller
392 * has an interrupt handler that can report if a Powered Device is connected
393 *
394 * Return: true if the power control of the power domain is managed by software,
395 * false otherwise
396 */
397static bool pse_pw_d_is_sw_pw_control(struct pse_controller_dev *pcdev,
398 struct pse_power_domain *pw_d)
399{
400 if (!pw_d)
401 return false;
402
403 if (pw_d->budget_eval_strategy == PSE_BUDGET_EVAL_STRAT_STATIC)
404 return true;
405 if (pw_d->budget_eval_strategy == PSE_BUDGET_EVAL_STRAT_DISABLED &&
406 pcdev->ops->pi_enable && pcdev->irq)
407 return true;
408
409 return false;
410}
411
412static int pse_pi_is_enabled(struct regulator_dev *rdev)
413{
414 struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
415 const struct pse_controller_ops *ops;
416 int id, ret;
417
418 ops = pcdev->ops;
419 if (!ops->pi_get_admin_state)
420 return -EOPNOTSUPP;
421
422 id = rdev_get_id(rdev);
423 mutex_lock(&pcdev->lock);
424 if (pse_pw_d_is_sw_pw_control(pcdev, pcdev->pi[id].pw_d)) {
425 ret = pcdev->pi[id].admin_state_enabled;
426 goto out;
427 }
428
429 ret = pse_pi_is_hw_enabled(pcdev, id);
430
431out:
432 mutex_unlock(&pcdev->lock);
433
434 return ret;
435}
436
437/**
438 * pse_pi_deallocate_pw_budget - Deallocate power budget of the PI
439 * @pi: a pointer to the PSE PI
440 */
441static void pse_pi_deallocate_pw_budget(struct pse_pi *pi)
442{
443 if (!pi->pw_d || !pi->pw_allocated_mW)
444 return;
445
446 regulator_free_power_budget(pi->pw_d->supply, pi->pw_allocated_mW);
447 pi->pw_allocated_mW = 0;
448}
449
450/**
451 * _pse_pi_disable - Call disable operation. Assumes the PSE lock has been
452 * acquired.
453 * @pcdev: a pointer to the PSE
454 * @id: index of the PSE control
455 *
456 * Return: 0 on success and failure value on error
457 */
458static int _pse_pi_disable(struct pse_controller_dev *pcdev, int id)
459{
460 const struct pse_controller_ops *ops = pcdev->ops;
461 int ret;
462
463 if (!ops->pi_disable)
464 return -EOPNOTSUPP;
465
466 ret = ops->pi_disable(pcdev, id);
467 if (ret)
468 return ret;
469
470 pse_pi_deallocate_pw_budget(&pcdev->pi[id]);
471
472 if (pse_pw_d_is_sw_pw_control(pcdev, pcdev->pi[id].pw_d))
473 pse_pw_d_retry_power_delivery(pcdev, pcdev->pi[id].pw_d);
474
475 return 0;
476}
477
478/**
479 * pse_disable_pi_pol - Disable a PI on a power budget policy
480 * @pcdev: a pointer to the PSE
481 * @id: index of the PSE PI
482 *
483 * Return: 0 on success and failure value on error
484 */
485static int pse_disable_pi_pol(struct pse_controller_dev *pcdev, int id)
486{
487 unsigned long notifs = ETHTOOL_PSE_EVENT_OVER_BUDGET;
488 struct pse_ntf ntf = {};
489 int ret;
490
491 dev_dbg(pcdev->dev, "Disabling PI %d to free power budget\n", id);
492
493 ret = _pse_pi_disable(pcdev, id);
494 if (ret)
495 notifs |= ETHTOOL_PSE_EVENT_SW_PW_CONTROL_ERROR;
496
497 ntf.notifs = notifs;
498 ntf.id = id;
499 kfifo_in_spinlocked(&pcdev->ntf_fifo, &ntf, 1, &pcdev->ntf_fifo_lock);
500 schedule_work(&pcdev->ntf_work);
501
502 return ret;
503}
504
505/**
506 * pse_disable_pi_prio - Disable all PIs of a given priority inside a PSE
507 * power domain
508 * @pcdev: a pointer to the PSE
509 * @pw_d: a pointer to the PSE power domain
510 * @prio: priority
511 *
512 * Return: 0 on success and failure value on error
513 */
514static int pse_disable_pi_prio(struct pse_controller_dev *pcdev,
515 struct pse_power_domain *pw_d,
516 int prio)
517{
518 int i;
519
520 for (i = 0; i < pcdev->nr_lines; i++) {
521 int ret;
522
523 if (pcdev->pi[i].prio != prio ||
524 pcdev->pi[i].pw_d != pw_d ||
525 pse_pi_is_hw_enabled(pcdev, i) <= 0)
526 continue;
527
528 ret = pse_disable_pi_pol(pcdev, i);
529 if (ret)
530 return ret;
531 }
532
533 return 0;
534}
535
536/**
537 * pse_pi_allocate_pw_budget_static_prio - Allocate power budget for the PI
538 * when the budget eval strategy is
539 * static
540 * @pcdev: a pointer to the PSE
541 * @id: index of the PSE control
542 * @pw_req: power requested in mW
543 * @extack: extack for error reporting
544 *
545 * Allocates power using static budget evaluation strategy, where allocation
546 * is based on PD classification. When insufficient budget is available,
547 * lower-priority ports (higher priority numbers) are turned off first.
548 *
549 * Return: 0 on success and failure value on error
550 */
551static int
552pse_pi_allocate_pw_budget_static_prio(struct pse_controller_dev *pcdev, int id,
553 int pw_req, struct netlink_ext_ack *extack)
554{
555 struct pse_pi *pi = &pcdev->pi[id];
556 int ret, _prio;
557
558 _prio = pcdev->nr_lines;
559 while (regulator_request_power_budget(pi->pw_d->supply, pw_req) == -ERANGE) {
560 if (_prio <= pi->prio) {
561 NL_SET_ERR_MSG_FMT(extack,
562 "PI %d: not enough power budget available",
563 id);
564 return -ERANGE;
565 }
566
567 ret = pse_disable_pi_prio(pcdev, pi->pw_d, _prio);
568 if (ret < 0)
569 return ret;
570
571 _prio--;
572 }
573
574 pi->pw_allocated_mW = pw_req;
575 return 0;
576}
577
578/**
579 * pse_pi_allocate_pw_budget - Allocate power budget for the PI
580 * @pcdev: a pointer to the PSE
581 * @id: index of the PSE control
582 * @pw_req: power requested in mW
583 * @extack: extack for error reporting
584 *
585 * Return: 0 on success and failure value on error
586 */
587static int pse_pi_allocate_pw_budget(struct pse_controller_dev *pcdev, int id,
588 int pw_req, struct netlink_ext_ack *extack)
589{
590 struct pse_pi *pi = &pcdev->pi[id];
591
592 if (!pi->pw_d)
593 return 0;
594
595 /* PSE_BUDGET_EVAL_STRAT_STATIC */
596 if (pi->pw_d->budget_eval_strategy == PSE_BUDGET_EVAL_STRAT_STATIC)
597 return pse_pi_allocate_pw_budget_static_prio(pcdev, id, pw_req,
598 extack);
599
600 return 0;
601}
602
603/**
604 * _pse_pi_delivery_power_sw_pw_ctrl - Enable PSE PI in case of software power
605 * control. Assumes the PSE lock has been
606 * acquired.
607 * @pcdev: a pointer to the PSE
608 * @id: index of the PSE control
609 * @extack: extack for error reporting
610 *
611 * Return: 0 on success and failure value on error
612 */
613static int _pse_pi_delivery_power_sw_pw_ctrl(struct pse_controller_dev *pcdev,
614 int id,
615 struct netlink_ext_ack *extack)
616{
617 const struct pse_controller_ops *ops = pcdev->ops;
618 struct pse_pi *pi = &pcdev->pi[id];
619 int ret, pw_req;
620
621 if (!ops->pi_get_pw_req) {
622 /* No power allocation management */
623 ret = ops->pi_enable(pcdev, id);
624 if (ret)
625 NL_SET_ERR_MSG_FMT(extack,
626 "PI %d: enable error %d",
627 id, ret);
628 return ret;
629 }
630
631 ret = ops->pi_get_pw_req(pcdev, id);
632 if (ret < 0)
633 return ret;
634
635 pw_req = ret;
636
637 /* Compare requested power with port power limit and use the lowest
638 * one.
639 */
640 if (ops->pi_get_pw_limit) {
641 ret = ops->pi_get_pw_limit(pcdev, id);
642 if (ret < 0)
643 return ret;
644
645 if (ret < pw_req)
646 pw_req = ret;
647 }
648
649 ret = pse_pi_allocate_pw_budget(pcdev, id, pw_req, extack);
650 if (ret)
651 return ret;
652
653 ret = ops->pi_enable(pcdev, id);
654 if (ret) {
655 pse_pi_deallocate_pw_budget(pi);
656 NL_SET_ERR_MSG_FMT(extack,
657 "PI %d: enable error %d",
658 id, ret);
659 return ret;
660 }
661
662 return 0;
663}
664
665static int pse_pi_enable(struct regulator_dev *rdev)
666{
667 struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
668 const struct pse_controller_ops *ops;
669 int id, ret = 0;
670
671 ops = pcdev->ops;
672 if (!ops->pi_enable)
673 return -EOPNOTSUPP;
674
675 id = rdev_get_id(rdev);
676 mutex_lock(&pcdev->lock);
677 if (pse_pw_d_is_sw_pw_control(pcdev, pcdev->pi[id].pw_d)) {
678 /* Manage enabled status by software.
679 * Real enable process will happen if a port is connected.
680 */
681 if (pcdev->pi[id].isr_pd_detected) {
682 struct netlink_ext_ack extack;
683
684 ret = _pse_pi_delivery_power_sw_pw_ctrl(pcdev, id, &extack);
685 }
686 if (!ret || ret == -ERANGE) {
687 pcdev->pi[id].admin_state_enabled = 1;
688 ret = 0;
689 }
690 mutex_unlock(&pcdev->lock);
691 return ret;
692 }
693
694 ret = ops->pi_enable(pcdev, id);
695 if (!ret)
696 pcdev->pi[id].admin_state_enabled = 1;
697 mutex_unlock(&pcdev->lock);
698
699 return ret;
700}
701
702static int pse_pi_disable(struct regulator_dev *rdev)
703{
704 struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
705 struct pse_pi *pi;
706 int id, ret;
707
708 id = rdev_get_id(rdev);
709 pi = &pcdev->pi[id];
710 mutex_lock(&pcdev->lock);
711 ret = _pse_pi_disable(pcdev, id);
712 if (!ret)
713 pi->admin_state_enabled = 0;
714
715 mutex_unlock(&pcdev->lock);
716 return 0;
717}
718
719static int _pse_pi_get_voltage(struct regulator_dev *rdev)
720{
721 struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
722 const struct pse_controller_ops *ops;
723 int id;
724
725 ops = pcdev->ops;
726 if (!ops->pi_get_voltage)
727 return -EOPNOTSUPP;
728
729 id = rdev_get_id(rdev);
730 return ops->pi_get_voltage(pcdev, id);
731}
732
733static int pse_pi_get_voltage(struct regulator_dev *rdev)
734{
735 struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
736 int ret;
737
738 mutex_lock(&pcdev->lock);
739 ret = _pse_pi_get_voltage(rdev);
740 mutex_unlock(&pcdev->lock);
741
742 return ret;
743}
744
745static int pse_pi_get_current_limit(struct regulator_dev *rdev)
746{
747 struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
748 const struct pse_controller_ops *ops;
749 int id, uV, mW, ret;
750 s64 tmp_64;
751
752 ops = pcdev->ops;
753 id = rdev_get_id(rdev);
754 if (!ops->pi_get_pw_limit || !ops->pi_get_voltage)
755 return -EOPNOTSUPP;
756
757 mutex_lock(&pcdev->lock);
758 ret = ops->pi_get_pw_limit(pcdev, id);
759 if (ret < 0)
760 goto out;
761 mW = ret;
762
763 ret = _pse_pi_get_voltage(rdev);
764 if (!ret) {
765 dev_err(pcdev->dev, "Voltage null\n");
766 ret = -ERANGE;
767 goto out;
768 }
769 if (ret < 0)
770 goto out;
771 uV = ret;
772
773 tmp_64 = mW;
774 tmp_64 *= 1000000000ull;
775 /* uA = mW * 1000000000 / uV */
776 ret = DIV_ROUND_CLOSEST_ULL(tmp_64, uV);
777
778out:
779 mutex_unlock(&pcdev->lock);
780 return ret;
781}
782
783static int pse_pi_set_current_limit(struct regulator_dev *rdev, int min_uA,
784 int max_uA)
785{
786 struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
787 const struct pse_controller_ops *ops;
788 int id, mW, ret;
789 s64 tmp_64;
790
791 ops = pcdev->ops;
792 if (!ops->pi_set_pw_limit || !ops->pi_get_voltage)
793 return -EOPNOTSUPP;
794
795 if (max_uA > MAX_PI_CURRENT)
796 return -ERANGE;
797
798 id = rdev_get_id(rdev);
799 mutex_lock(&pcdev->lock);
800 ret = _pse_pi_get_voltage(rdev);
801 if (!ret) {
802 dev_err(pcdev->dev, "Voltage null\n");
803 ret = -ERANGE;
804 goto out;
805 }
806 if (ret < 0)
807 goto out;
808
809 tmp_64 = ret;
810 tmp_64 *= max_uA;
811 /* mW = uA * uV / 1000000000 */
812 mW = DIV_ROUND_CLOSEST_ULL(tmp_64, 1000000000);
813 ret = ops->pi_set_pw_limit(pcdev, id, mW);
814out:
815 mutex_unlock(&pcdev->lock);
816
817 return ret;
818}
819
820static const struct regulator_ops pse_pi_ops = {
821 .is_enabled = pse_pi_is_enabled,
822 .enable = pse_pi_enable,
823 .disable = pse_pi_disable,
824 .get_voltage = pse_pi_get_voltage,
825 .get_current_limit = pse_pi_get_current_limit,
826 .set_current_limit = pse_pi_set_current_limit,
827};
828
829static int
830devm_pse_pi_regulator_register(struct pse_controller_dev *pcdev,
831 char *name, int id)
832{
833 struct regulator_init_data *rinit_data;
834 struct regulator_config rconfig = {0};
835 struct regulator_desc *rdesc;
836 struct regulator_dev *rdev;
837
838 rinit_data = devm_kzalloc(pcdev->dev, sizeof(*rinit_data),
839 GFP_KERNEL);
840 if (!rinit_data)
841 return -ENOMEM;
842
843 rdesc = devm_kzalloc(pcdev->dev, sizeof(*rdesc), GFP_KERNEL);
844 if (!rdesc)
845 return -ENOMEM;
846
847 /* Regulator descriptor id have to be the same as its associated
848 * PSE PI id for the well functioning of the PSE controls.
849 */
850 rdesc->id = id;
851 rdesc->name = name;
852 rdesc->type = REGULATOR_VOLTAGE;
853 rdesc->ops = &pse_pi_ops;
854 rdesc->owner = pcdev->owner;
855
856 rinit_data->constraints.valid_ops_mask = REGULATOR_CHANGE_STATUS;
857
858 if (pcdev->ops->pi_set_pw_limit)
859 rinit_data->constraints.valid_ops_mask |=
860 REGULATOR_CHANGE_CURRENT;
861
862 rinit_data->supply_regulator = "vpwr";
863
864 rconfig.dev = pcdev->dev;
865 rconfig.driver_data = pcdev;
866 rconfig.init_data = rinit_data;
867 rconfig.of_node = pcdev->pi[id].np;
868
869 rdev = devm_regulator_register(pcdev->dev, rdesc, &rconfig);
870 if (IS_ERR(rdev)) {
871 dev_err_probe(pcdev->dev, PTR_ERR(rdev),
872 "Failed to register regulator\n");
873 return PTR_ERR(rdev);
874 }
875
876 pcdev->pi[id].rdev = rdev;
877
878 return 0;
879}
880
881static void __pse_pw_d_release(struct kref *kref)
882{
883 struct pse_power_domain *pw_d = container_of(kref,
884 struct pse_power_domain,
885 refcnt);
886
887 regulator_put(pw_d->supply);
888 xa_erase(&pse_pw_d_map, pw_d->id);
889 mutex_unlock(&pse_pw_d_mutex);
890}
891
892/**
893 * pse_flush_pw_ds - flush all PSE power domains of a PSE
894 * @pcdev: a pointer to the initialized PSE controller device
895 */
896static void pse_flush_pw_ds(struct pse_controller_dev *pcdev)
897{
898 struct pse_power_domain *pw_d;
899 int i;
900
901 for (i = 0; i < pcdev->nr_lines; i++) {
902 if (!pcdev->pi[i].pw_d)
903 continue;
904
905 pw_d = xa_load(&pse_pw_d_map, pcdev->pi[i].pw_d->id);
906 if (!pw_d)
907 continue;
908
909 kref_put_mutex(&pw_d->refcnt, __pse_pw_d_release,
910 &pse_pw_d_mutex);
911 }
912}
913
914/**
915 * devm_pse_alloc_pw_d - allocate a new PSE power domain for a device
916 * @dev: device that is registering this PSE power domain
917 *
918 * Return: Pointer to the newly allocated PSE power domain or error pointers
919 */
920static struct pse_power_domain *devm_pse_alloc_pw_d(struct device *dev)
921{
922 struct pse_power_domain *pw_d;
923 int index, ret;
924
925 pw_d = devm_kzalloc(dev, sizeof(*pw_d), GFP_KERNEL);
926 if (!pw_d)
927 return ERR_PTR(-ENOMEM);
928
929 ret = xa_alloc(&pse_pw_d_map, &index, pw_d, XA_LIMIT(1, PSE_PW_D_LIMIT),
930 GFP_KERNEL);
931 if (ret)
932 return ERR_PTR(ret);
933
934 kref_init(&pw_d->refcnt);
935 pw_d->id = index;
936 return pw_d;
937}
938
939/**
940 * pse_register_pw_ds - register the PSE power domains for a PSE
941 * @pcdev: a pointer to the PSE controller device
942 *
943 * Return: 0 on success and failure value on error
944 */
945static int pse_register_pw_ds(struct pse_controller_dev *pcdev)
946{
947 int i, ret = 0;
948
949 mutex_lock(&pse_pw_d_mutex);
950 for (i = 0; i < pcdev->nr_lines; i++) {
951 struct regulator_dev *rdev = pcdev->pi[i].rdev;
952 struct pse_power_domain *pw_d;
953 struct regulator *supply;
954 bool present = false;
955 unsigned long index;
956
957 /* No regulator or regulator parent supply registered.
958 * We need a regulator parent to register a PSE power domain
959 */
960 if (!rdev || !rdev->supply)
961 continue;
962
963 xa_for_each(&pse_pw_d_map, index, pw_d) {
964 /* Power supply already registered as a PSE power
965 * domain.
966 */
967 if (regulator_is_equal(pw_d->supply, rdev->supply)) {
968 present = true;
969 pcdev->pi[i].pw_d = pw_d;
970 break;
971 }
972 }
973 if (present) {
974 kref_get(&pw_d->refcnt);
975 continue;
976 }
977
978 pw_d = devm_pse_alloc_pw_d(pcdev->dev);
979 if (IS_ERR(pw_d)) {
980 ret = PTR_ERR(pw_d);
981 goto out;
982 }
983
984 supply = regulator_get(&rdev->dev, rdev->supply_name);
985 if (IS_ERR(supply)) {
986 xa_erase(&pse_pw_d_map, pw_d->id);
987 ret = PTR_ERR(supply);
988 goto out;
989 }
990
991 pw_d->supply = supply;
992 if (pcdev->supp_budget_eval_strategies)
993 pw_d->budget_eval_strategy = pcdev->supp_budget_eval_strategies;
994 else
995 pw_d->budget_eval_strategy = PSE_BUDGET_EVAL_STRAT_DISABLED;
996 kref_init(&pw_d->refcnt);
997 pcdev->pi[i].pw_d = pw_d;
998 }
999
1000out:
1001 mutex_unlock(&pse_pw_d_mutex);
1002 return ret;
1003}
1004
1005/**
1006 * pse_send_ntf_worker - Worker to send PSE notifications
1007 * @work: work object
1008 *
1009 * Manage and send PSE netlink notifications using a workqueue to avoid
1010 * deadlock between pcdev_lock and pse_list_mutex.
1011 */
1012static void pse_send_ntf_worker(struct work_struct *work)
1013{
1014 struct pse_controller_dev *pcdev;
1015 struct pse_ntf ntf;
1016
1017 pcdev = container_of(work, struct pse_controller_dev, ntf_work);
1018
1019 while (kfifo_out(&pcdev->ntf_fifo, &ntf, 1)) {
1020 struct net_device *netdev;
1021 struct pse_control *psec;
1022
1023 psec = pse_control_find_by_id(pcdev, ntf.id);
1024 rtnl_lock();
1025 netdev = pse_control_get_netdev(psec);
1026 if (netdev)
1027 ethnl_pse_send_ntf(netdev, ntf.notifs);
1028 rtnl_unlock();
1029 pse_control_put(psec);
1030 }
1031}
1032
1033/**
1034 * pse_controller_register - register a PSE controller device
1035 * @pcdev: a pointer to the initialized PSE controller device
1036 *
1037 * Return: 0 on success and failure value on error
1038 */
1039int pse_controller_register(struct pse_controller_dev *pcdev)
1040{
1041 size_t reg_name_len;
1042 int ret, i;
1043
1044 mutex_init(&pcdev->lock);
1045 INIT_LIST_HEAD(&pcdev->pse_control_head);
1046 spin_lock_init(&pcdev->ntf_fifo_lock);
1047 ret = kfifo_alloc(&pcdev->ntf_fifo, pcdev->nr_lines, GFP_KERNEL);
1048 if (ret) {
1049 dev_err(pcdev->dev, "failed to allocate kfifo notifications\n");
1050 return ret;
1051 }
1052 INIT_WORK(&pcdev->ntf_work, pse_send_ntf_worker);
1053
1054 if (!pcdev->nr_lines)
1055 pcdev->nr_lines = 1;
1056
1057 if (!pcdev->ops->pi_get_admin_state ||
1058 !pcdev->ops->pi_get_pw_status) {
1059 dev_err(pcdev->dev,
1060 "Mandatory status report callbacks are missing");
1061 return -EINVAL;
1062 }
1063
1064 ret = of_load_pse_pis(pcdev);
1065 if (ret)
1066 return ret;
1067
1068 if (pcdev->ops->setup_pi_matrix) {
1069 ret = pcdev->ops->setup_pi_matrix(pcdev);
1070 if (ret)
1071 return ret;
1072 }
1073
1074 /* Each regulator name len is pcdev dev name + 7 char +
1075 * int max digit number (10) + 1
1076 */
1077 reg_name_len = strlen(dev_name(pcdev->dev)) + 18;
1078
1079 /* Register PI regulators */
1080 for (i = 0; i < pcdev->nr_lines; i++) {
1081 char *reg_name;
1082
1083 /* Do not register regulator for PIs not described */
1084 if (!pcdev->no_of_pse_pi && !pcdev->pi[i].np)
1085 continue;
1086
1087 reg_name = devm_kzalloc(pcdev->dev, reg_name_len, GFP_KERNEL);
1088 if (!reg_name)
1089 return -ENOMEM;
1090
1091 snprintf(reg_name, reg_name_len, "pse-%s_pi%d",
1092 dev_name(pcdev->dev), i);
1093
1094 ret = devm_pse_pi_regulator_register(pcdev, reg_name, i);
1095 if (ret)
1096 return ret;
1097 }
1098
1099 ret = pse_register_pw_ds(pcdev);
1100 if (ret)
1101 return ret;
1102
1103 mutex_lock(&pse_list_mutex);
1104 list_add(&pcdev->list, &pse_controller_list);
1105 mutex_unlock(&pse_list_mutex);
1106
1107 return 0;
1108}
1109EXPORT_SYMBOL_GPL(pse_controller_register);
1110
1111/**
1112 * pse_controller_unregister - unregister a PSE controller device
1113 * @pcdev: a pointer to the PSE controller device
1114 */
1115void pse_controller_unregister(struct pse_controller_dev *pcdev)
1116{
1117 pse_flush_pw_ds(pcdev);
1118 pse_release_pis(pcdev);
1119 if (pcdev->irq)
1120 disable_irq(pcdev->irq);
1121 cancel_work_sync(&pcdev->ntf_work);
1122 kfifo_free(&pcdev->ntf_fifo);
1123 mutex_lock(&pse_list_mutex);
1124 list_del(&pcdev->list);
1125 mutex_unlock(&pse_list_mutex);
1126}
1127EXPORT_SYMBOL_GPL(pse_controller_unregister);
1128
1129static void devm_pse_controller_release(struct device *dev, void *res)
1130{
1131 pse_controller_unregister(*(struct pse_controller_dev **)res);
1132}
1133
1134/**
1135 * devm_pse_controller_register - resource managed pse_controller_register()
1136 * @dev: device that is registering this PSE controller
1137 * @pcdev: a pointer to the initialized PSE controller device
1138 *
1139 * Managed pse_controller_register(). For PSE controllers registered by
1140 * this function, pse_controller_unregister() is automatically called on
1141 * driver detach. See pse_controller_register() for more information.
1142 *
1143 * Return: 0 on success and failure value on error
1144 */
1145int devm_pse_controller_register(struct device *dev,
1146 struct pse_controller_dev *pcdev)
1147{
1148 struct pse_controller_dev **pcdevp;
1149 int ret;
1150
1151 pcdevp = devres_alloc(devm_pse_controller_release, sizeof(*pcdevp),
1152 GFP_KERNEL);
1153 if (!pcdevp)
1154 return -ENOMEM;
1155
1156 ret = pse_controller_register(pcdev);
1157 if (ret) {
1158 devres_free(pcdevp);
1159 return ret;
1160 }
1161
1162 *pcdevp = pcdev;
1163 devres_add(dev, pcdevp);
1164
1165 return 0;
1166}
1167EXPORT_SYMBOL_GPL(devm_pse_controller_register);
1168
1169struct pse_irq {
1170 struct pse_controller_dev *pcdev;
1171 struct pse_irq_desc desc;
1172 unsigned long *notifs;
1173 unsigned long *notifs_mask;
1174};
1175
1176/**
1177 * pse_to_regulator_notifs - Convert PSE notifications to Regulator
1178 * notifications
1179 * @notifs: PSE notifications
1180 *
1181 * Return: Regulator notifications
1182 */
1183static unsigned long pse_to_regulator_notifs(unsigned long notifs)
1184{
1185 unsigned long rnotifs = 0;
1186
1187 if (notifs & ETHTOOL_PSE_EVENT_OVER_CURRENT)
1188 rnotifs |= REGULATOR_EVENT_OVER_CURRENT;
1189 if (notifs & ETHTOOL_PSE_EVENT_OVER_TEMP)
1190 rnotifs |= REGULATOR_EVENT_OVER_TEMP;
1191
1192 return rnotifs;
1193}
1194
1195/**
1196 * pse_set_config_isr - Set PSE control config according to the PSE
1197 * notifications
1198 * @pcdev: a pointer to the PSE
1199 * @id: index of the PSE control
1200 * @notifs: PSE event notifications
1201 *
1202 * Return: 0 on success and failure value on error
1203 */
1204static int pse_set_config_isr(struct pse_controller_dev *pcdev, int id,
1205 unsigned long notifs)
1206{
1207 int ret = 0;
1208
1209 if (notifs & PSE_BUDGET_EVAL_STRAT_DYNAMIC)
1210 return 0;
1211
1212 if ((notifs & ETHTOOL_C33_PSE_EVENT_DISCONNECTION) &&
1213 ((notifs & ETHTOOL_C33_PSE_EVENT_DETECTION) ||
1214 (notifs & ETHTOOL_C33_PSE_EVENT_CLASSIFICATION))) {
1215 dev_dbg(pcdev->dev,
1216 "PI %d: error, connection and disconnection reported simultaneously",
1217 id);
1218 return -EINVAL;
1219 }
1220
1221 if (notifs & ETHTOOL_C33_PSE_EVENT_CLASSIFICATION) {
1222 struct netlink_ext_ack extack;
1223
1224 pcdev->pi[id].isr_pd_detected = true;
1225 if (pcdev->pi[id].admin_state_enabled) {
1226 ret = _pse_pi_delivery_power_sw_pw_ctrl(pcdev, id,
1227 &extack);
1228 if (ret == -ERANGE)
1229 ret = 0;
1230 }
1231 } else if (notifs & ETHTOOL_C33_PSE_EVENT_DISCONNECTION) {
1232 if (pcdev->pi[id].admin_state_enabled &&
1233 pcdev->pi[id].isr_pd_detected)
1234 ret = _pse_pi_disable(pcdev, id);
1235 pcdev->pi[id].isr_pd_detected = false;
1236 }
1237
1238 return ret;
1239}
1240
1241/**
1242 * pse_isr - IRQ handler for PSE
1243 * @irq: irq number
1244 * @data: pointer to user interrupt structure
1245 *
1246 * Return: irqreturn_t - status of IRQ
1247 */
1248static irqreturn_t pse_isr(int irq, void *data)
1249{
1250 struct pse_controller_dev *pcdev;
1251 struct pse_irq_desc *desc;
1252 struct pse_irq *h = data;
1253 int ret, i;
1254
1255 desc = &h->desc;
1256 pcdev = h->pcdev;
1257
1258 /* Clear notifs mask */
1259 memset(h->notifs, 0, pcdev->nr_lines * sizeof(*h->notifs));
1260 bitmap_zero(h->notifs_mask, pcdev->nr_lines);
1261 mutex_lock(&pcdev->lock);
1262 ret = desc->map_event(irq, pcdev, h->notifs, h->notifs_mask);
1263 if (ret || bitmap_empty(h->notifs_mask, pcdev->nr_lines)) {
1264 mutex_unlock(&pcdev->lock);
1265 return IRQ_NONE;
1266 }
1267
1268 for_each_set_bit(i, h->notifs_mask, pcdev->nr_lines) {
1269 unsigned long notifs, rnotifs;
1270 struct pse_ntf ntf = {};
1271
1272 /* Do nothing PI not described */
1273 if (!pcdev->pi[i].rdev)
1274 continue;
1275
1276 notifs = h->notifs[i];
1277 if (pse_pw_d_is_sw_pw_control(pcdev, pcdev->pi[i].pw_d)) {
1278 ret = pse_set_config_isr(pcdev, i, notifs);
1279 if (ret)
1280 notifs |= ETHTOOL_PSE_EVENT_SW_PW_CONTROL_ERROR;
1281 }
1282
1283 dev_dbg(h->pcdev->dev,
1284 "Sending PSE notification EVT 0x%lx\n", notifs);
1285
1286 ntf.notifs = notifs;
1287 ntf.id = i;
1288 kfifo_in_spinlocked(&pcdev->ntf_fifo, &ntf, 1,
1289 &pcdev->ntf_fifo_lock);
1290 schedule_work(&pcdev->ntf_work);
1291
1292 rnotifs = pse_to_regulator_notifs(notifs);
1293 regulator_notifier_call_chain(pcdev->pi[i].rdev, rnotifs,
1294 NULL);
1295 }
1296
1297 mutex_unlock(&pcdev->lock);
1298
1299 return IRQ_HANDLED;
1300}
1301
1302/**
1303 * devm_pse_irq_helper - Register IRQ based PSE event notifier
1304 * @pcdev: a pointer to the PSE
1305 * @irq: the irq value to be passed to request_irq
1306 * @irq_flags: the flags to be passed to request_irq
1307 * @d: PSE interrupt description
1308 *
1309 * Return: 0 on success and errno on failure
1310 */
1311int devm_pse_irq_helper(struct pse_controller_dev *pcdev, int irq,
1312 int irq_flags, const struct pse_irq_desc *d)
1313{
1314 struct device *dev = pcdev->dev;
1315 size_t irq_name_len;
1316 struct pse_irq *h;
1317 char *irq_name;
1318 int ret;
1319
1320 if (!d || !d->map_event || !d->name)
1321 return -EINVAL;
1322
1323 h = devm_kzalloc(dev, sizeof(*h), GFP_KERNEL);
1324 if (!h)
1325 return -ENOMEM;
1326
1327 h->pcdev = pcdev;
1328 h->desc = *d;
1329
1330 /* IRQ name len is pcdev dev name + 5 char + irq desc name + 1 */
1331 irq_name_len = strlen(dev_name(pcdev->dev)) + 5 + strlen(d->name) + 1;
1332 irq_name = devm_kzalloc(dev, irq_name_len, GFP_KERNEL);
1333 if (!irq_name)
1334 return -ENOMEM;
1335
1336 snprintf(irq_name, irq_name_len, "pse-%s:%s", dev_name(pcdev->dev),
1337 d->name);
1338
1339 h->notifs = devm_kcalloc(dev, pcdev->nr_lines,
1340 sizeof(*h->notifs), GFP_KERNEL);
1341 if (!h->notifs)
1342 return -ENOMEM;
1343
1344 h->notifs_mask = devm_bitmap_zalloc(dev, pcdev->nr_lines, GFP_KERNEL);
1345 if (!h->notifs_mask)
1346 return -ENOMEM;
1347
1348 ret = devm_request_threaded_irq(dev, irq, NULL, pse_isr,
1349 IRQF_ONESHOT | irq_flags,
1350 irq_name, h);
1351 if (ret)
1352 dev_err(pcdev->dev, "Failed to request IRQ %d\n", irq);
1353
1354 pcdev->irq = irq;
1355 return ret;
1356}
1357EXPORT_SYMBOL_GPL(devm_pse_irq_helper);
1358
1359/* PSE control section */
1360
1361static void __pse_control_release(struct kref *kref)
1362{
1363 struct pse_control *psec = container_of(kref, struct pse_control,
1364 refcnt);
1365
1366 lockdep_assert_held(&pse_list_mutex);
1367
1368 if (psec->pcdev->pi[psec->id].admin_state_enabled)
1369 regulator_disable(psec->ps);
1370 regulator_put(psec->ps);
1371
1372 module_put(psec->pcdev->owner);
1373
1374 list_del(&psec->list);
1375 kfree(psec);
1376}
1377
1378static void __pse_control_put_internal(struct pse_control *psec)
1379{
1380 lockdep_assert_held(&pse_list_mutex);
1381
1382 kref_put(&psec->refcnt, __pse_control_release);
1383}
1384
1385/**
1386 * pse_control_put - free the PSE control
1387 * @psec: PSE control pointer
1388 */
1389void pse_control_put(struct pse_control *psec)
1390{
1391 if (IS_ERR_OR_NULL(psec))
1392 return;
1393
1394 mutex_lock(&pse_list_mutex);
1395 __pse_control_put_internal(psec);
1396 mutex_unlock(&pse_list_mutex);
1397}
1398EXPORT_SYMBOL_GPL(pse_control_put);
1399
1400static struct pse_control *
1401pse_control_get_internal(struct pse_controller_dev *pcdev, unsigned int index,
1402 struct phy_device *phydev)
1403{
1404 struct pse_control *psec;
1405 int ret;
1406
1407 lockdep_assert_held(&pse_list_mutex);
1408
1409 list_for_each_entry(psec, &pcdev->pse_control_head, list) {
1410 if (psec->id == index) {
1411 kref_get(&psec->refcnt);
1412 return psec;
1413 }
1414 }
1415
1416 psec = kzalloc_obj(*psec);
1417 if (!psec)
1418 return ERR_PTR(-ENOMEM);
1419
1420 if (!try_module_get(pcdev->owner)) {
1421 ret = -ENODEV;
1422 goto free_psec;
1423 }
1424
1425 if (!pcdev->ops->pi_get_admin_state) {
1426 ret = -EOPNOTSUPP;
1427 goto free_psec;
1428 }
1429
1430 /* Initialize admin_state_enabled before the regulator_get. This
1431 * aims to have the right value reported in the first is_enabled
1432 * call in case of control managed by software.
1433 */
1434 ret = pse_pi_is_hw_enabled(pcdev, index);
1435 if (ret < 0)
1436 goto free_psec;
1437
1438 pcdev->pi[index].admin_state_enabled = ret;
1439 psec->ps = regulator_get_exclusive(pcdev->dev,
1440 rdev_get_name(pcdev->pi[index].rdev));
1441 if (IS_ERR(psec->ps)) {
1442 ret = PTR_ERR(psec->ps);
1443 goto put_module;
1444 }
1445
1446 psec->pcdev = pcdev;
1447 list_add(&psec->list, &pcdev->pse_control_head);
1448 psec->id = index;
1449 psec->attached_phydev = phydev;
1450 kref_init(&psec->refcnt);
1451
1452 return psec;
1453
1454put_module:
1455 module_put(pcdev->owner);
1456free_psec:
1457 kfree(psec);
1458
1459 return ERR_PTR(ret);
1460}
1461
1462/**
1463 * of_pse_match_pi - Find the PSE PI id matching the device node phandle
1464 * @pcdev: a pointer to the PSE controller device
1465 * @np: a pointer to the device node
1466 *
1467 * Return: id of the PSE PI, -EINVAL if not found
1468 */
1469static int of_pse_match_pi(struct pse_controller_dev *pcdev,
1470 struct device_node *np)
1471{
1472 int i;
1473
1474 for (i = 0; i < pcdev->nr_lines; i++) {
1475 if (pcdev->pi[i].np == np)
1476 return i;
1477 }
1478
1479 return -EINVAL;
1480}
1481
1482/**
1483 * psec_id_xlate - translate pse_spec to the PSE line number according
1484 * to the number of pse-cells in case of no pse_pi node
1485 * @pcdev: a pointer to the PSE controller device
1486 * @pse_spec: PSE line specifier as found in the device tree
1487 *
1488 * Return: 0 if #pse-cells = <0>. Return PSE line number otherwise.
1489 */
1490static int psec_id_xlate(struct pse_controller_dev *pcdev,
1491 const struct of_phandle_args *pse_spec)
1492{
1493 if (!pcdev->of_pse_n_cells)
1494 return 0;
1495
1496 if (pcdev->of_pse_n_cells > 1 ||
1497 pse_spec->args[0] >= pcdev->nr_lines)
1498 return -EINVAL;
1499
1500 return pse_spec->args[0];
1501}
1502
1503struct pse_control *of_pse_control_get(struct device_node *node,
1504 struct phy_device *phydev)
1505{
1506 struct pse_controller_dev *r, *pcdev;
1507 struct of_phandle_args args;
1508 struct pse_control *psec;
1509 int psec_id;
1510 int ret;
1511
1512 if (!node)
1513 return ERR_PTR(-EINVAL);
1514
1515 ret = of_parse_phandle_with_args(node, "pses", "#pse-cells", 0, &args);
1516 if (ret)
1517 return ERR_PTR(ret);
1518
1519 mutex_lock(&pse_list_mutex);
1520 pcdev = NULL;
1521 list_for_each_entry(r, &pse_controller_list, list) {
1522 if (!r->no_of_pse_pi) {
1523 ret = of_pse_match_pi(r, args.np);
1524 if (ret >= 0) {
1525 pcdev = r;
1526 psec_id = ret;
1527 break;
1528 }
1529 } else if (args.np == r->dev->of_node) {
1530 pcdev = r;
1531 break;
1532 }
1533 }
1534
1535 if (!pcdev) {
1536 psec = ERR_PTR(-EPROBE_DEFER);
1537 goto out;
1538 }
1539
1540 if (WARN_ON(args.args_count != pcdev->of_pse_n_cells)) {
1541 psec = ERR_PTR(-EINVAL);
1542 goto out;
1543 }
1544
1545 if (pcdev->no_of_pse_pi) {
1546 psec_id = psec_id_xlate(pcdev, &args);
1547 if (psec_id < 0) {
1548 psec = ERR_PTR(psec_id);
1549 goto out;
1550 }
1551 }
1552
1553 /* pse_list_mutex also protects the pcdev's pse_control list */
1554 psec = pse_control_get_internal(pcdev, psec_id, phydev);
1555
1556out:
1557 mutex_unlock(&pse_list_mutex);
1558 of_node_put(args.np);
1559
1560 return psec;
1561}
1562EXPORT_SYMBOL_GPL(of_pse_control_get);
1563
1564/**
1565 * pse_get_sw_admin_state - Convert the software admin state to c33 or podl
1566 * admin state value used in the standard
1567 * @psec: PSE control pointer
1568 * @admin_state: a pointer to the admin_state structure
1569 */
1570static void pse_get_sw_admin_state(struct pse_control *psec,
1571 struct pse_admin_state *admin_state)
1572{
1573 struct pse_pi *pi = &psec->pcdev->pi[psec->id];
1574
1575 if (pse_has_podl(psec)) {
1576 if (pi->admin_state_enabled)
1577 admin_state->podl_admin_state =
1578 ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED;
1579 else
1580 admin_state->podl_admin_state =
1581 ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED;
1582 }
1583 if (pse_has_c33(psec)) {
1584 if (pi->admin_state_enabled)
1585 admin_state->c33_admin_state =
1586 ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED;
1587 else
1588 admin_state->c33_admin_state =
1589 ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED;
1590 }
1591}
1592
1593/**
1594 * pse_ethtool_get_status - get status of PSE control
1595 * @psec: PSE control pointer
1596 * @extack: extack for reporting useful error messages
1597 * @status: struct to store PSE status
1598 *
1599 * Return: 0 on success and failure value on error
1600 */
1601int pse_ethtool_get_status(struct pse_control *psec,
1602 struct netlink_ext_ack *extack,
1603 struct ethtool_pse_control_status *status)
1604{
1605 struct pse_admin_state admin_state = {0};
1606 struct pse_pw_status pw_status = {0};
1607 const struct pse_controller_ops *ops;
1608 struct pse_controller_dev *pcdev;
1609 struct pse_pi *pi;
1610 int ret;
1611
1612 pcdev = psec->pcdev;
1613 ops = pcdev->ops;
1614
1615 pi = &pcdev->pi[psec->id];
1616 mutex_lock(&pcdev->lock);
1617 if (pi->pw_d) {
1618 status->pw_d_id = pi->pw_d->id;
1619 if (pse_pw_d_is_sw_pw_control(pcdev, pi->pw_d)) {
1620 pse_get_sw_admin_state(psec, &admin_state);
1621 } else {
1622 ret = ops->pi_get_admin_state(pcdev, psec->id,
1623 &admin_state);
1624 if (ret)
1625 goto out;
1626 }
1627 status->podl_admin_state = admin_state.podl_admin_state;
1628 status->c33_admin_state = admin_state.c33_admin_state;
1629
1630 switch (pi->pw_d->budget_eval_strategy) {
1631 case PSE_BUDGET_EVAL_STRAT_STATIC:
1632 status->prio_max = pcdev->nr_lines - 1;
1633 status->prio = pi->prio;
1634 break;
1635 case PSE_BUDGET_EVAL_STRAT_DYNAMIC:
1636 status->prio_max = pcdev->pis_prio_max;
1637 if (ops->pi_get_prio) {
1638 ret = ops->pi_get_prio(pcdev, psec->id);
1639 if (ret < 0)
1640 goto out;
1641
1642 status->prio = ret;
1643 }
1644 break;
1645 default:
1646 break;
1647 }
1648 }
1649
1650 ret = ops->pi_get_pw_status(pcdev, psec->id, &pw_status);
1651 if (ret)
1652 goto out;
1653 status->podl_pw_status = pw_status.podl_pw_status;
1654 status->c33_pw_status = pw_status.c33_pw_status;
1655
1656 if (ops->pi_get_ext_state) {
1657 struct pse_ext_state_info ext_state_info = {0};
1658
1659 ret = ops->pi_get_ext_state(pcdev, psec->id,
1660 &ext_state_info);
1661 if (ret)
1662 goto out;
1663
1664 memcpy(&status->c33_ext_state_info,
1665 &ext_state_info.c33_ext_state_info,
1666 sizeof(status->c33_ext_state_info));
1667 }
1668
1669 if (ops->pi_get_pw_class) {
1670 ret = ops->pi_get_pw_class(pcdev, psec->id);
1671 if (ret < 0)
1672 goto out;
1673
1674 status->c33_pw_class = ret;
1675 }
1676
1677 if (ops->pi_get_actual_pw) {
1678 ret = ops->pi_get_actual_pw(pcdev, psec->id);
1679 if (ret < 0)
1680 goto out;
1681
1682 status->c33_actual_pw = ret;
1683 }
1684
1685 if (ops->pi_get_pw_limit) {
1686 ret = ops->pi_get_pw_limit(pcdev, psec->id);
1687 if (ret < 0)
1688 goto out;
1689
1690 status->c33_avail_pw_limit = ret;
1691 }
1692
1693 if (ops->pi_get_pw_limit_ranges) {
1694 struct pse_pw_limit_ranges pw_limit_ranges = {0};
1695
1696 ret = ops->pi_get_pw_limit_ranges(pcdev, psec->id,
1697 &pw_limit_ranges);
1698 if (ret < 0)
1699 goto out;
1700
1701 status->c33_pw_limit_ranges =
1702 pw_limit_ranges.c33_pw_limit_ranges;
1703 status->c33_pw_limit_nb_ranges = ret;
1704 }
1705out:
1706 mutex_unlock(&psec->pcdev->lock);
1707 return ret;
1708}
1709EXPORT_SYMBOL_GPL(pse_ethtool_get_status);
1710
1711static int pse_ethtool_c33_set_config(struct pse_control *psec,
1712 const struct pse_control_config *config)
1713{
1714 int err = 0;
1715
1716 /* Look at admin_state_enabled status to not call regulator_enable
1717 * or regulator_disable twice creating a regulator counter mismatch
1718 */
1719 switch (config->c33_admin_control) {
1720 case ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED:
1721 /* We could have mismatch between admin_state_enabled and
1722 * state reported by regulator_is_enabled. This can occur when
1723 * the PI is forcibly turn off by the controller. Call
1724 * regulator_disable on that case to fix the counters state.
1725 */
1726 if (psec->pcdev->pi[psec->id].admin_state_enabled &&
1727 !regulator_is_enabled(psec->ps)) {
1728 err = regulator_disable(psec->ps);
1729 if (err)
1730 break;
1731 }
1732 if (!psec->pcdev->pi[psec->id].admin_state_enabled)
1733 err = regulator_enable(psec->ps);
1734 break;
1735 case ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED:
1736 if (psec->pcdev->pi[psec->id].admin_state_enabled)
1737 err = regulator_disable(psec->ps);
1738 break;
1739 default:
1740 err = -EOPNOTSUPP;
1741 }
1742
1743 return err;
1744}
1745
1746static int pse_ethtool_podl_set_config(struct pse_control *psec,
1747 const struct pse_control_config *config)
1748{
1749 int err = 0;
1750
1751 /* Look at admin_state_enabled status to not call regulator_enable
1752 * or regulator_disable twice creating a regulator counter mismatch
1753 */
1754 switch (config->podl_admin_control) {
1755 case ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED:
1756 if (!psec->pcdev->pi[psec->id].admin_state_enabled)
1757 err = regulator_enable(psec->ps);
1758 break;
1759 case ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED:
1760 if (psec->pcdev->pi[psec->id].admin_state_enabled)
1761 err = regulator_disable(psec->ps);
1762 break;
1763 default:
1764 err = -EOPNOTSUPP;
1765 }
1766
1767 return err;
1768}
1769
1770/**
1771 * pse_ethtool_set_config - set PSE control configuration
1772 * @psec: PSE control pointer
1773 * @extack: extack for reporting useful error messages
1774 * @config: Configuration of the test to run
1775 *
1776 * Return: 0 on success and failure value on error
1777 */
1778int pse_ethtool_set_config(struct pse_control *psec,
1779 struct netlink_ext_ack *extack,
1780 const struct pse_control_config *config)
1781{
1782 int err = 0;
1783
1784 if (pse_has_c33(psec) && config->c33_admin_control) {
1785 err = pse_ethtool_c33_set_config(psec, config);
1786 if (err)
1787 return err;
1788 }
1789
1790 if (pse_has_podl(psec) && config->podl_admin_control)
1791 err = pse_ethtool_podl_set_config(psec, config);
1792
1793 return err;
1794}
1795EXPORT_SYMBOL_GPL(pse_ethtool_set_config);
1796
1797/**
1798 * pse_pi_update_pw_budget - Update PSE power budget allocated with new
1799 * power in mW
1800 * @pcdev: a pointer to the PSE controller device
1801 * @id: index of the PSE PI
1802 * @pw_req: power requested
1803 * @extack: extack for reporting useful error messages
1804 *
1805 * Return: Previous power allocated on success and failure value on error
1806 */
1807static int pse_pi_update_pw_budget(struct pse_controller_dev *pcdev, int id,
1808 const unsigned int pw_req,
1809 struct netlink_ext_ack *extack)
1810{
1811 struct pse_pi *pi = &pcdev->pi[id];
1812 int previous_pw_allocated;
1813 int pw_diff, ret = 0;
1814
1815 /* We don't want pw_allocated_mW value change in the middle of an
1816 * power budget update
1817 */
1818 mutex_lock(&pcdev->lock);
1819 previous_pw_allocated = pi->pw_allocated_mW;
1820 pw_diff = pw_req - previous_pw_allocated;
1821 if (!pw_diff) {
1822 goto out;
1823 } else if (pw_diff > 0) {
1824 ret = regulator_request_power_budget(pi->pw_d->supply, pw_diff);
1825 if (ret) {
1826 NL_SET_ERR_MSG_FMT(extack,
1827 "PI %d: not enough power budget available",
1828 id);
1829 goto out;
1830 }
1831
1832 } else {
1833 regulator_free_power_budget(pi->pw_d->supply, -pw_diff);
1834 }
1835 pi->pw_allocated_mW = pw_req;
1836 ret = previous_pw_allocated;
1837
1838out:
1839 mutex_unlock(&pcdev->lock);
1840 return ret;
1841}
1842
1843/**
1844 * pse_ethtool_set_pw_limit - set PSE control power limit
1845 * @psec: PSE control pointer
1846 * @extack: extack for reporting useful error messages
1847 * @pw_limit: power limit value in mW
1848 *
1849 * Return: 0 on success and failure value on error
1850 */
1851int pse_ethtool_set_pw_limit(struct pse_control *psec,
1852 struct netlink_ext_ack *extack,
1853 const unsigned int pw_limit)
1854{
1855 int uV, uA, ret, previous_pw_allocated = 0;
1856 s64 tmp_64;
1857
1858 if (pw_limit > MAX_PI_PW)
1859 return -ERANGE;
1860
1861 ret = regulator_get_voltage(psec->ps);
1862 if (!ret) {
1863 NL_SET_ERR_MSG(extack,
1864 "Can't calculate the current, PSE voltage read is 0");
1865 return -ERANGE;
1866 }
1867 if (ret < 0) {
1868 NL_SET_ERR_MSG(extack,
1869 "Error reading PSE voltage");
1870 return ret;
1871 }
1872 uV = ret;
1873
1874 tmp_64 = pw_limit;
1875 tmp_64 *= 1000000000ull;
1876 /* uA = mW * 1000000000 / uV */
1877 uA = DIV_ROUND_CLOSEST_ULL(tmp_64, uV);
1878
1879 /* Update power budget only in software power control case and
1880 * if a Power Device is powered.
1881 */
1882 if (pse_pw_d_is_sw_pw_control(psec->pcdev,
1883 psec->pcdev->pi[psec->id].pw_d) &&
1884 psec->pcdev->pi[psec->id].admin_state_enabled &&
1885 psec->pcdev->pi[psec->id].isr_pd_detected) {
1886 ret = pse_pi_update_pw_budget(psec->pcdev, psec->id,
1887 pw_limit, extack);
1888 if (ret < 0)
1889 return ret;
1890 previous_pw_allocated = ret;
1891 }
1892
1893 ret = regulator_set_current_limit(psec->ps, 0, uA);
1894 if (ret < 0 && previous_pw_allocated) {
1895 pse_pi_update_pw_budget(psec->pcdev, psec->id,
1896 previous_pw_allocated, extack);
1897 }
1898
1899 return ret;
1900}
1901EXPORT_SYMBOL_GPL(pse_ethtool_set_pw_limit);
1902
1903/**
1904 * pse_ethtool_set_prio - Set PSE PI priority according to the budget
1905 * evaluation strategy
1906 * @psec: PSE control pointer
1907 * @extack: extack for reporting useful error messages
1908 * @prio: priovity value
1909 *
1910 * Return: 0 on success and failure value on error
1911 */
1912int pse_ethtool_set_prio(struct pse_control *psec,
1913 struct netlink_ext_ack *extack,
1914 unsigned int prio)
1915{
1916 struct pse_controller_dev *pcdev = psec->pcdev;
1917 const struct pse_controller_ops *ops;
1918 int ret = 0;
1919
1920 if (!pcdev->pi[psec->id].pw_d) {
1921 NL_SET_ERR_MSG(extack, "no power domain attached");
1922 return -EOPNOTSUPP;
1923 }
1924
1925 /* We don't want priority change in the middle of an
1926 * enable/disable call or a priority mode change
1927 */
1928 mutex_lock(&pcdev->lock);
1929 switch (pcdev->pi[psec->id].pw_d->budget_eval_strategy) {
1930 case PSE_BUDGET_EVAL_STRAT_STATIC:
1931 if (prio >= pcdev->nr_lines) {
1932 NL_SET_ERR_MSG_FMT(extack,
1933 "priority %d exceed priority max %d",
1934 prio, pcdev->nr_lines);
1935 ret = -ERANGE;
1936 goto out;
1937 }
1938
1939 pcdev->pi[psec->id].prio = prio;
1940 pse_pw_d_retry_power_delivery(pcdev, pcdev->pi[psec->id].pw_d);
1941 break;
1942
1943 case PSE_BUDGET_EVAL_STRAT_DYNAMIC:
1944 ops = psec->pcdev->ops;
1945 if (!ops->pi_set_prio) {
1946 NL_SET_ERR_MSG(extack,
1947 "pse driver does not support setting port priority");
1948 ret = -EOPNOTSUPP;
1949 goto out;
1950 }
1951
1952 if (prio > pcdev->pis_prio_max) {
1953 NL_SET_ERR_MSG_FMT(extack,
1954 "priority %d exceed priority max %d",
1955 prio, pcdev->pis_prio_max);
1956 ret = -ERANGE;
1957 goto out;
1958 }
1959
1960 ret = ops->pi_set_prio(pcdev, psec->id, prio);
1961 break;
1962
1963 default:
1964 ret = -EOPNOTSUPP;
1965 }
1966
1967out:
1968 mutex_unlock(&pcdev->lock);
1969 return ret;
1970}
1971EXPORT_SYMBOL_GPL(pse_ethtool_set_prio);
1972
1973bool pse_has_podl(struct pse_control *psec)
1974{
1975 return psec->pcdev->types & ETHTOOL_PSE_PODL;
1976}
1977EXPORT_SYMBOL_GPL(pse_has_podl);
1978
1979bool pse_has_c33(struct pse_control *psec)
1980{
1981 return psec->pcdev->types & ETHTOOL_PSE_C33;
1982}
1983EXPORT_SYMBOL_GPL(pse_has_c33);