pse_core.c

   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/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 = kcalloc(pcdev->nr_lines, sizeof(*pcdev->pi), GFP_KERNEL);
 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_net_by_id - Find net attached to the pse control 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};
1174
1175/**
1176 * pse_to_regulator_notifs - Convert PSE notifications to Regulator
1177 *			     notifications
1178 * @notifs: PSE notifications
1179 *
1180 * Return: Regulator notifications
1181 */
1182static unsigned long pse_to_regulator_notifs(unsigned long notifs)
1183{
1184	unsigned long rnotifs = 0;
1185
1186	if (notifs & ETHTOOL_PSE_EVENT_OVER_CURRENT)
1187		rnotifs |= REGULATOR_EVENT_OVER_CURRENT;
1188	if (notifs & ETHTOOL_PSE_EVENT_OVER_TEMP)
1189		rnotifs |= REGULATOR_EVENT_OVER_TEMP;
1190
1191	return rnotifs;
1192}
1193
1194/**
1195 * pse_set_config_isr - Set PSE control config according to the PSE
1196 *			notifications
1197 * @pcdev: a pointer to the PSE
1198 * @id: index of the PSE control
1199 * @notifs: PSE event notifications
1200 *
1201 * Return: 0 on success and failure value on error
1202 */
1203static int pse_set_config_isr(struct pse_controller_dev *pcdev, int id,
1204			      unsigned long notifs)
1205{
1206	int ret = 0;
1207
1208	if (notifs & PSE_BUDGET_EVAL_STRAT_DYNAMIC)
1209		return 0;
1210
1211	if ((notifs & ETHTOOL_C33_PSE_EVENT_DISCONNECTION) &&
1212	    ((notifs & ETHTOOL_C33_PSE_EVENT_DETECTION) ||
1213	     (notifs & ETHTOOL_C33_PSE_EVENT_CLASSIFICATION))) {
1214		dev_dbg(pcdev->dev,
1215			"PI %d: error, connection and disconnection reported simultaneously",
1216			id);
1217		return -EINVAL;
1218	}
1219
1220	if (notifs & ETHTOOL_C33_PSE_EVENT_CLASSIFICATION) {
1221		struct netlink_ext_ack extack;
1222
1223		pcdev->pi[id].isr_pd_detected = true;
1224		if (pcdev->pi[id].admin_state_enabled) {
1225			ret = _pse_pi_delivery_power_sw_pw_ctrl(pcdev, id,
1226								&extack);
1227			if (ret == -ERANGE)
1228				ret = 0;
1229		}
1230	} else if (notifs & ETHTOOL_C33_PSE_EVENT_DISCONNECTION) {
1231		if (pcdev->pi[id].admin_state_enabled &&
1232		    pcdev->pi[id].isr_pd_detected)
1233			ret = _pse_pi_disable(pcdev, id);
1234		pcdev->pi[id].isr_pd_detected = false;
1235	}
1236
1237	return ret;
1238}
1239
1240/**
1241 * pse_isr - IRQ handler for PSE
1242 * @irq: irq number
1243 * @data: pointer to user interrupt structure
1244 *
1245 * Return: irqreturn_t - status of IRQ
1246 */
1247static irqreturn_t pse_isr(int irq, void *data)
1248{
1249	struct pse_controller_dev *pcdev;
1250	unsigned long notifs_mask = 0;
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	mutex_lock(&pcdev->lock);
1261	ret = desc->map_event(irq, pcdev, h->notifs, &notifs_mask);
1262	if (ret || !notifs_mask) {
1263		mutex_unlock(&pcdev->lock);
1264		return IRQ_NONE;
1265	}
1266
1267	for_each_set_bit(i, &notifs_mask, pcdev->nr_lines) {
1268		unsigned long notifs, rnotifs;
1269		struct pse_ntf ntf = {};
1270
1271		/* Do nothing PI not described */
1272		if (!pcdev->pi[i].rdev)
1273			continue;
1274
1275		notifs = h->notifs[i];
1276		if (pse_pw_d_is_sw_pw_control(pcdev, pcdev->pi[i].pw_d)) {
1277			ret = pse_set_config_isr(pcdev, i, notifs);
1278			if (ret)
1279				notifs |= ETHTOOL_PSE_EVENT_SW_PW_CONTROL_ERROR;
1280		}
1281
1282		dev_dbg(h->pcdev->dev,
1283			"Sending PSE notification EVT 0x%lx\n", notifs);
1284
1285		ntf.notifs = notifs;
1286		ntf.id = i;
1287		kfifo_in_spinlocked(&pcdev->ntf_fifo, &ntf, 1,
1288				    &pcdev->ntf_fifo_lock);
1289		schedule_work(&pcdev->ntf_work);
1290
1291		rnotifs = pse_to_regulator_notifs(notifs);
1292		regulator_notifier_call_chain(pcdev->pi[i].rdev, rnotifs,
1293					      NULL);
1294	}
1295
1296	mutex_unlock(&pcdev->lock);
1297
1298	return IRQ_HANDLED;
1299}
1300
1301/**
1302 * devm_pse_irq_helper - Register IRQ based PSE event notifier
1303 * @pcdev: a pointer to the PSE
1304 * @irq: the irq value to be passed to request_irq
1305 * @irq_flags: the flags to be passed to request_irq
1306 * @d: PSE interrupt description
1307 *
1308 * Return: 0 on success and errno on failure
1309 */
1310int devm_pse_irq_helper(struct pse_controller_dev *pcdev, int irq,
1311			int irq_flags, const struct pse_irq_desc *d)
1312{
1313	struct device *dev = pcdev->dev;
1314	size_t irq_name_len;
1315	struct pse_irq *h;
1316	char *irq_name;
1317	int ret;
1318
1319	if (!d || !d->map_event || !d->name)
1320		return -EINVAL;
1321
1322	h = devm_kzalloc(dev, sizeof(*h), GFP_KERNEL);
1323	if (!h)
1324		return -ENOMEM;
1325
1326	h->pcdev = pcdev;
1327	h->desc = *d;
1328
1329	/* IRQ name len is pcdev dev name + 5 char + irq desc name + 1 */
1330	irq_name_len = strlen(dev_name(pcdev->dev)) + 5 + strlen(d->name) + 1;
1331	irq_name = devm_kzalloc(dev, irq_name_len, GFP_KERNEL);
1332	if (!irq_name)
1333		return -ENOMEM;
1334
1335	snprintf(irq_name, irq_name_len, "pse-%s:%s", dev_name(pcdev->dev),
1336		 d->name);
1337
1338	h->notifs = devm_kcalloc(dev, pcdev->nr_lines,
1339				 sizeof(*h->notifs), GFP_KERNEL);
1340	if (!h->notifs)
1341		return -ENOMEM;
1342
1343	ret = devm_request_threaded_irq(dev, irq, NULL, pse_isr,
1344					IRQF_ONESHOT | irq_flags,
1345					irq_name, h);
1346	if (ret)
1347		dev_err(pcdev->dev, "Failed to request IRQ %d\n", irq);
1348
1349	pcdev->irq = irq;
1350	return ret;
1351}
1352EXPORT_SYMBOL_GPL(devm_pse_irq_helper);
1353
1354/* PSE control section */
1355
1356static void __pse_control_release(struct kref *kref)
1357{
1358	struct pse_control *psec = container_of(kref, struct pse_control,
1359						  refcnt);
1360
1361	lockdep_assert_held(&pse_list_mutex);
1362
1363	if (psec->pcdev->pi[psec->id].admin_state_enabled)
1364		regulator_disable(psec->ps);
1365	devm_regulator_put(psec->ps);
1366
1367	module_put(psec->pcdev->owner);
1368
1369	list_del(&psec->list);
1370	kfree(psec);
1371}
1372
1373static void __pse_control_put_internal(struct pse_control *psec)
1374{
1375	lockdep_assert_held(&pse_list_mutex);
1376
1377	kref_put(&psec->refcnt, __pse_control_release);
1378}
1379
1380/**
1381 * pse_control_put - free the PSE control
1382 * @psec: PSE control pointer
1383 */
1384void pse_control_put(struct pse_control *psec)
1385{
1386	if (IS_ERR_OR_NULL(psec))
1387		return;
1388
1389	mutex_lock(&pse_list_mutex);
1390	__pse_control_put_internal(psec);
1391	mutex_unlock(&pse_list_mutex);
1392}
1393EXPORT_SYMBOL_GPL(pse_control_put);
1394
1395static struct pse_control *
1396pse_control_get_internal(struct pse_controller_dev *pcdev, unsigned int index,
1397			 struct phy_device *phydev)
1398{
1399	struct pse_control *psec;
1400	int ret;
1401
1402	lockdep_assert_held(&pse_list_mutex);
1403
1404	list_for_each_entry(psec, &pcdev->pse_control_head, list) {
1405		if (psec->id == index) {
1406			kref_get(&psec->refcnt);
1407			return psec;
1408		}
1409	}
1410
1411	psec = kzalloc(sizeof(*psec), GFP_KERNEL);
1412	if (!psec)
1413		return ERR_PTR(-ENOMEM);
1414
1415	if (!try_module_get(pcdev->owner)) {
1416		ret = -ENODEV;
1417		goto free_psec;
1418	}
1419
1420	if (!pcdev->ops->pi_get_admin_state) {
1421		ret = -EOPNOTSUPP;
1422		goto free_psec;
1423	}
1424
1425	/* Initialize admin_state_enabled before the regulator_get. This
1426	 * aims to have the right value reported in the first is_enabled
1427	 * call in case of control managed by software.
1428	 */
1429	ret = pse_pi_is_hw_enabled(pcdev, index);
1430	if (ret < 0)
1431		goto free_psec;
1432
1433	pcdev->pi[index].admin_state_enabled = ret;
1434	psec->ps = devm_regulator_get_exclusive(pcdev->dev,
1435						rdev_get_name(pcdev->pi[index].rdev));
1436	if (IS_ERR(psec->ps)) {
1437		ret = PTR_ERR(psec->ps);
1438		goto put_module;
1439	}
1440
1441	psec->pcdev = pcdev;
1442	list_add(&psec->list, &pcdev->pse_control_head);
1443	psec->id = index;
1444	psec->attached_phydev = phydev;
1445	kref_init(&psec->refcnt);
1446
1447	return psec;
1448
1449put_module:
1450	module_put(pcdev->owner);
1451free_psec:
1452	kfree(psec);
1453
1454	return ERR_PTR(ret);
1455}
1456
1457/**
1458 * of_pse_match_pi - Find the PSE PI id matching the device node phandle
1459 * @pcdev: a pointer to the PSE controller device
1460 * @np: a pointer to the device node
1461 *
1462 * Return: id of the PSE PI, -EINVAL if not found
1463 */
1464static int of_pse_match_pi(struct pse_controller_dev *pcdev,
1465			   struct device_node *np)
1466{
1467	int i;
1468
1469	for (i = 0; i < pcdev->nr_lines; i++) {
1470		if (pcdev->pi[i].np == np)
1471			return i;
1472	}
1473
1474	return -EINVAL;
1475}
1476
1477/**
1478 * psec_id_xlate - translate pse_spec to the PSE line number according
1479 *		   to the number of pse-cells in case of no pse_pi node
1480 * @pcdev: a pointer to the PSE controller device
1481 * @pse_spec: PSE line specifier as found in the device tree
1482 *
1483 * Return: 0 if #pse-cells = <0>. Return PSE line number otherwise.
1484 */
1485static int psec_id_xlate(struct pse_controller_dev *pcdev,
1486			 const struct of_phandle_args *pse_spec)
1487{
1488	if (!pcdev->of_pse_n_cells)
1489		return 0;
1490
1491	if (pcdev->of_pse_n_cells > 1 ||
1492	    pse_spec->args[0] >= pcdev->nr_lines)
1493		return -EINVAL;
1494
1495	return pse_spec->args[0];
1496}
1497
1498struct pse_control *of_pse_control_get(struct device_node *node,
1499				       struct phy_device *phydev)
1500{
1501	struct pse_controller_dev *r, *pcdev;
1502	struct of_phandle_args args;
1503	struct pse_control *psec;
1504	int psec_id;
1505	int ret;
1506
1507	if (!node)
1508		return ERR_PTR(-EINVAL);
1509
1510	ret = of_parse_phandle_with_args(node, "pses", "#pse-cells", 0, &args);
1511	if (ret)
1512		return ERR_PTR(ret);
1513
1514	mutex_lock(&pse_list_mutex);
1515	pcdev = NULL;
1516	list_for_each_entry(r, &pse_controller_list, list) {
1517		if (!r->no_of_pse_pi) {
1518			ret = of_pse_match_pi(r, args.np);
1519			if (ret >= 0) {
1520				pcdev = r;
1521				psec_id = ret;
1522				break;
1523			}
1524		} else if (args.np == r->dev->of_node) {
1525			pcdev = r;
1526			break;
1527		}
1528	}
1529
1530	if (!pcdev) {
1531		psec = ERR_PTR(-EPROBE_DEFER);
1532		goto out;
1533	}
1534
1535	if (WARN_ON(args.args_count != pcdev->of_pse_n_cells)) {
1536		psec = ERR_PTR(-EINVAL);
1537		goto out;
1538	}
1539
1540	if (pcdev->no_of_pse_pi) {
1541		psec_id = psec_id_xlate(pcdev, &args);
1542		if (psec_id < 0) {
1543			psec = ERR_PTR(psec_id);
1544			goto out;
1545		}
1546	}
1547
1548	/* pse_list_mutex also protects the pcdev's pse_control list */
1549	psec = pse_control_get_internal(pcdev, psec_id, phydev);
1550
1551out:
1552	mutex_unlock(&pse_list_mutex);
1553	of_node_put(args.np);
1554
1555	return psec;
1556}
1557EXPORT_SYMBOL_GPL(of_pse_control_get);
1558
1559/**
1560 * pse_get_sw_admin_state - Convert the software admin state to c33 or podl
1561 *			    admin state value used in the standard
1562 * @psec: PSE control pointer
1563 * @admin_state: a pointer to the admin_state structure
1564 */
1565static void pse_get_sw_admin_state(struct pse_control *psec,
1566				   struct pse_admin_state *admin_state)
1567{
1568	struct pse_pi *pi = &psec->pcdev->pi[psec->id];
1569
1570	if (pse_has_podl(psec)) {
1571		if (pi->admin_state_enabled)
1572			admin_state->podl_admin_state =
1573				ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED;
1574		else
1575			admin_state->podl_admin_state =
1576				ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED;
1577	}
1578	if (pse_has_c33(psec)) {
1579		if (pi->admin_state_enabled)
1580			admin_state->c33_admin_state =
1581				ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED;
1582		else
1583			admin_state->c33_admin_state =
1584				ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED;
1585	}
1586}
1587
1588/**
1589 * pse_ethtool_get_status - get status of PSE control
1590 * @psec: PSE control pointer
1591 * @extack: extack for reporting useful error messages
1592 * @status: struct to store PSE status
1593 *
1594 * Return: 0 on success and failure value on error
1595 */
1596int pse_ethtool_get_status(struct pse_control *psec,
1597			   struct netlink_ext_ack *extack,
1598			   struct ethtool_pse_control_status *status)
1599{
1600	struct pse_admin_state admin_state = {0};
1601	struct pse_pw_status pw_status = {0};
1602	const struct pse_controller_ops *ops;
1603	struct pse_controller_dev *pcdev;
1604	struct pse_pi *pi;
1605	int ret;
1606
1607	pcdev = psec->pcdev;
1608	ops = pcdev->ops;
1609
1610	pi = &pcdev->pi[psec->id];
1611	mutex_lock(&pcdev->lock);
1612	if (pi->pw_d) {
1613		status->pw_d_id = pi->pw_d->id;
1614		if (pse_pw_d_is_sw_pw_control(pcdev, pi->pw_d)) {
1615			pse_get_sw_admin_state(psec, &admin_state);
1616		} else {
1617			ret = ops->pi_get_admin_state(pcdev, psec->id,
1618						      &admin_state);
1619			if (ret)
1620				goto out;
1621		}
1622		status->podl_admin_state = admin_state.podl_admin_state;
1623		status->c33_admin_state = admin_state.c33_admin_state;
1624
1625		switch (pi->pw_d->budget_eval_strategy) {
1626		case PSE_BUDGET_EVAL_STRAT_STATIC:
1627			status->prio_max = pcdev->nr_lines - 1;
1628			status->prio = pi->prio;
1629			break;
1630		case PSE_BUDGET_EVAL_STRAT_DYNAMIC:
1631			status->prio_max = pcdev->pis_prio_max;
1632			if (ops->pi_get_prio) {
1633				ret = ops->pi_get_prio(pcdev, psec->id);
1634				if (ret < 0)
1635					goto out;
1636
1637				status->prio = ret;
1638			}
1639			break;
1640		default:
1641			break;
1642		}
1643	}
1644
1645	ret = ops->pi_get_pw_status(pcdev, psec->id, &pw_status);
1646	if (ret)
1647		goto out;
1648	status->podl_pw_status = pw_status.podl_pw_status;
1649	status->c33_pw_status = pw_status.c33_pw_status;
1650
1651	if (ops->pi_get_ext_state) {
1652		struct pse_ext_state_info ext_state_info = {0};
1653
1654		ret = ops->pi_get_ext_state(pcdev, psec->id,
1655					    &ext_state_info);
1656		if (ret)
1657			goto out;
1658
1659		memcpy(&status->c33_ext_state_info,
1660		       &ext_state_info.c33_ext_state_info,
1661		       sizeof(status->c33_ext_state_info));
1662	}
1663
1664	if (ops->pi_get_pw_class) {
1665		ret = ops->pi_get_pw_class(pcdev, psec->id);
1666		if (ret < 0)
1667			goto out;
1668
1669		status->c33_pw_class = ret;
1670	}
1671
1672	if (ops->pi_get_actual_pw) {
1673		ret = ops->pi_get_actual_pw(pcdev, psec->id);
1674		if (ret < 0)
1675			goto out;
1676
1677		status->c33_actual_pw = ret;
1678	}
1679
1680	if (ops->pi_get_pw_limit) {
1681		ret = ops->pi_get_pw_limit(pcdev, psec->id);
1682		if (ret < 0)
1683			goto out;
1684
1685		status->c33_avail_pw_limit = ret;
1686	}
1687
1688	if (ops->pi_get_pw_limit_ranges) {
1689		struct pse_pw_limit_ranges pw_limit_ranges = {0};
1690
1691		ret = ops->pi_get_pw_limit_ranges(pcdev, psec->id,
1692						  &pw_limit_ranges);
1693		if (ret < 0)
1694			goto out;
1695
1696		status->c33_pw_limit_ranges =
1697			pw_limit_ranges.c33_pw_limit_ranges;
1698		status->c33_pw_limit_nb_ranges = ret;
1699	}
1700out:
1701	mutex_unlock(&psec->pcdev->lock);
1702	return ret;
1703}
1704EXPORT_SYMBOL_GPL(pse_ethtool_get_status);
1705
1706static int pse_ethtool_c33_set_config(struct pse_control *psec,
1707				      const struct pse_control_config *config)
1708{
1709	int err = 0;
1710
1711	/* Look at admin_state_enabled status to not call regulator_enable
1712	 * or regulator_disable twice creating a regulator counter mismatch
1713	 */
1714	switch (config->c33_admin_control) {
1715	case ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED:
1716		/* We could have mismatch between admin_state_enabled and
1717		 * state reported by regulator_is_enabled. This can occur when
1718		 * the PI is forcibly turn off by the controller. Call
1719		 * regulator_disable on that case to fix the counters state.
1720		 */
1721		if (psec->pcdev->pi[psec->id].admin_state_enabled &&
1722		    !regulator_is_enabled(psec->ps)) {
1723			err = regulator_disable(psec->ps);
1724			if (err)
1725				break;
1726		}
1727		if (!psec->pcdev->pi[psec->id].admin_state_enabled)
1728			err = regulator_enable(psec->ps);
1729		break;
1730	case ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED:
1731		if (psec->pcdev->pi[psec->id].admin_state_enabled)
1732			err = regulator_disable(psec->ps);
1733		break;
1734	default:
1735		err = -EOPNOTSUPP;
1736	}
1737
1738	return err;
1739}
1740
1741static int pse_ethtool_podl_set_config(struct pse_control *psec,
1742				       const struct pse_control_config *config)
1743{
1744	int err = 0;
1745
1746	/* Look at admin_state_enabled status to not call regulator_enable
1747	 * or regulator_disable twice creating a regulator counter mismatch
1748	 */
1749	switch (config->podl_admin_control) {
1750	case ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED:
1751		if (!psec->pcdev->pi[psec->id].admin_state_enabled)
1752			err = regulator_enable(psec->ps);
1753		break;
1754	case ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED:
1755		if (psec->pcdev->pi[psec->id].admin_state_enabled)
1756			err = regulator_disable(psec->ps);
1757		break;
1758	default:
1759		err = -EOPNOTSUPP;
1760	}
1761
1762	return err;
1763}
1764
1765/**
1766 * pse_ethtool_set_config - set PSE control configuration
1767 * @psec: PSE control pointer
1768 * @extack: extack for reporting useful error messages
1769 * @config: Configuration of the test to run
1770 *
1771 * Return: 0 on success and failure value on error
1772 */
1773int pse_ethtool_set_config(struct pse_control *psec,
1774			   struct netlink_ext_ack *extack,
1775			   const struct pse_control_config *config)
1776{
1777	int err = 0;
1778
1779	if (pse_has_c33(psec) && config->c33_admin_control) {
1780		err = pse_ethtool_c33_set_config(psec, config);
1781		if (err)
1782			return err;
1783	}
1784
1785	if (pse_has_podl(psec) && config->podl_admin_control)
1786		err = pse_ethtool_podl_set_config(psec, config);
1787
1788	return err;
1789}
1790EXPORT_SYMBOL_GPL(pse_ethtool_set_config);
1791
1792/**
1793 * pse_pi_update_pw_budget - Update PSE power budget allocated with new
1794 *			     power in mW
1795 * @pcdev: a pointer to the PSE controller device
1796 * @id: index of the PSE PI
1797 * @pw_req: power requested
1798 * @extack: extack for reporting useful error messages
1799 *
1800 * Return: Previous power allocated on success and failure value on error
1801 */
1802static int pse_pi_update_pw_budget(struct pse_controller_dev *pcdev, int id,
1803				   const unsigned int pw_req,
1804				   struct netlink_ext_ack *extack)
1805{
1806	struct pse_pi *pi = &pcdev->pi[id];
1807	int previous_pw_allocated;
1808	int pw_diff, ret = 0;
1809
1810	/* We don't want pw_allocated_mW value change in the middle of an
1811	 * power budget update
1812	 */
1813	mutex_lock(&pcdev->lock);
1814	previous_pw_allocated = pi->pw_allocated_mW;
1815	pw_diff = pw_req - previous_pw_allocated;
1816	if (!pw_diff) {
1817		goto out;
1818	} else if (pw_diff > 0) {
1819		ret = regulator_request_power_budget(pi->pw_d->supply, pw_diff);
1820		if (ret) {
1821			NL_SET_ERR_MSG_FMT(extack,
1822					   "PI %d: not enough power budget available",
1823					   id);
1824			goto out;
1825		}
1826
1827	} else {
1828		regulator_free_power_budget(pi->pw_d->supply, -pw_diff);
1829	}
1830	pi->pw_allocated_mW = pw_req;
1831	ret = previous_pw_allocated;
1832
1833out:
1834	mutex_unlock(&pcdev->lock);
1835	return ret;
1836}
1837
1838/**
1839 * pse_ethtool_set_pw_limit - set PSE control power limit
1840 * @psec: PSE control pointer
1841 * @extack: extack for reporting useful error messages
1842 * @pw_limit: power limit value in mW
1843 *
1844 * Return: 0 on success and failure value on error
1845 */
1846int pse_ethtool_set_pw_limit(struct pse_control *psec,
1847			     struct netlink_ext_ack *extack,
1848			     const unsigned int pw_limit)
1849{
1850	int uV, uA, ret, previous_pw_allocated = 0;
1851	s64 tmp_64;
1852
1853	if (pw_limit > MAX_PI_PW)
1854		return -ERANGE;
1855
1856	ret = regulator_get_voltage(psec->ps);
1857	if (!ret) {
1858		NL_SET_ERR_MSG(extack,
1859			       "Can't calculate the current, PSE voltage read is 0");
1860		return -ERANGE;
1861	}
1862	if (ret < 0) {
1863		NL_SET_ERR_MSG(extack,
1864			       "Error reading PSE voltage");
1865		return ret;
1866	}
1867	uV = ret;
1868
1869	tmp_64 = pw_limit;
1870	tmp_64 *= 1000000000ull;
1871	/* uA = mW * 1000000000 / uV */
1872	uA = DIV_ROUND_CLOSEST_ULL(tmp_64, uV);
1873
1874	/* Update power budget only in software power control case and
1875	 * if a Power Device is powered.
1876	 */
1877	if (pse_pw_d_is_sw_pw_control(psec->pcdev,
1878				      psec->pcdev->pi[psec->id].pw_d) &&
1879	    psec->pcdev->pi[psec->id].admin_state_enabled &&
1880	    psec->pcdev->pi[psec->id].isr_pd_detected) {
1881		ret = pse_pi_update_pw_budget(psec->pcdev, psec->id,
1882					      pw_limit, extack);
1883		if (ret < 0)
1884			return ret;
1885		previous_pw_allocated = ret;
1886	}
1887
1888	ret = regulator_set_current_limit(psec->ps, 0, uA);
1889	if (ret < 0 && previous_pw_allocated) {
1890		pse_pi_update_pw_budget(psec->pcdev, psec->id,
1891					previous_pw_allocated, extack);
1892	}
1893
1894	return ret;
1895}
1896EXPORT_SYMBOL_GPL(pse_ethtool_set_pw_limit);
1897
1898/**
1899 * pse_ethtool_set_prio - Set PSE PI priority according to the budget
1900 *			  evaluation strategy
1901 * @psec: PSE control pointer
1902 * @extack: extack for reporting useful error messages
1903 * @prio: priovity value
1904 *
1905 * Return: 0 on success and failure value on error
1906 */
1907int pse_ethtool_set_prio(struct pse_control *psec,
1908			 struct netlink_ext_ack *extack,
1909			 unsigned int prio)
1910{
1911	struct pse_controller_dev *pcdev = psec->pcdev;
1912	const struct pse_controller_ops *ops;
1913	int ret = 0;
1914
1915	if (!pcdev->pi[psec->id].pw_d) {
1916		NL_SET_ERR_MSG(extack, "no power domain attached");
1917		return -EOPNOTSUPP;
1918	}
1919
1920	/* We don't want priority change in the middle of an
1921	 * enable/disable call or a priority mode change
1922	 */
1923	mutex_lock(&pcdev->lock);
1924	switch (pcdev->pi[psec->id].pw_d->budget_eval_strategy) {
1925	case PSE_BUDGET_EVAL_STRAT_STATIC:
1926		if (prio >= pcdev->nr_lines) {
1927			NL_SET_ERR_MSG_FMT(extack,
1928					   "priority %d exceed priority max %d",
1929					   prio, pcdev->nr_lines);
1930			ret = -ERANGE;
1931			goto out;
1932		}
1933
1934		pcdev->pi[psec->id].prio = prio;
1935		pse_pw_d_retry_power_delivery(pcdev, pcdev->pi[psec->id].pw_d);
1936		break;
1937
1938	case PSE_BUDGET_EVAL_STRAT_DYNAMIC:
1939		ops = psec->pcdev->ops;
1940		if (!ops->pi_set_prio) {
1941			NL_SET_ERR_MSG(extack,
1942				       "pse driver does not support setting port priority");
1943			ret = -EOPNOTSUPP;
1944			goto out;
1945		}
1946
1947		if (prio > pcdev->pis_prio_max) {
1948			NL_SET_ERR_MSG_FMT(extack,
1949					   "priority %d exceed priority max %d",
1950					   prio, pcdev->pis_prio_max);
1951			ret = -ERANGE;
1952			goto out;
1953		}
1954
1955		ret = ops->pi_set_prio(pcdev, psec->id, prio);
1956		break;
1957
1958	default:
1959		ret = -EOPNOTSUPP;
1960	}
1961
1962out:
1963	mutex_unlock(&pcdev->lock);
1964	return ret;
1965}
1966EXPORT_SYMBOL_GPL(pse_ethtool_set_prio);
1967
1968bool pse_has_podl(struct pse_control *psec)
1969{
1970	return psec->pcdev->types & ETHTOOL_PSE_PODL;
1971}
1972EXPORT_SYMBOL_GPL(pse_has_podl);
1973
1974bool pse_has_c33(struct pse_control *psec)
1975{
1976	return psec->pcdev->types & ETHTOOL_PSE_C33;
1977}
1978EXPORT_SYMBOL_GPL(pse_has_c33);