drivers/net/pse-pd/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/of.h>
  10 #include <linux/pse-pd/pse.h>
  11 #include <linux/regulator/driver.h>
  12 #include <linux/regulator/machine.h>
  13
  14 static DEFINE_MUTEX(pse_list_mutex);
  15 static LIST_HEAD(pse_controller_list);
  16
  17 /**
  18  * struct pse_control - a PSE control
  19  * @pcdev: a pointer to the PSE controller device
  20  *         this PSE control belongs to
  21  * @ps: PSE PI supply of the PSE control
  22  * @list: list entry for the pcdev's PSE controller list
  23  * @id: ID of the PSE line in the PSE controller device
  24  * @refcnt: Number of gets of this pse_control
  25  */
  26 struct pse_control {
  27         struct pse_controller_dev *pcdev;
  28         struct regulator *ps;
  29         struct list_head list;
  30         unsigned int id;
  31         struct kref refcnt;
  32 };
  33
  34 static int of_load_single_pse_pi_pairset(struct device_node *node,
  35                                          struct pse_pi *pi,
  36                                          int pairset_num)
  37 {
  38         struct device_node *pairset_np;
  39         const char *name;
  40         int ret;
  41
  42         ret = of_property_read_string_index(node, "pairset-names",
  43                                             pairset_num, &name);
  44         if (ret)
  45                 return ret;
  46
  47         if (!strcmp(name, "alternative-a")) {
  48                 pi->pairset[pairset_num].pinout = ALTERNATIVE_A;
  49         } else if (!strcmp(name, "alternative-b")) {
  50                 pi->pairset[pairset_num].pinout = ALTERNATIVE_B;
  51         } else {
  52                 pr_err("pse: wrong pairset-names value %s (%pOF)\n",
  53                        name, node);
  54                 return -EINVAL;
  55         }
  56
  57         pairset_np = of_parse_phandle(node, "pairsets", pairset_num);
  58         if (!pairset_np)
  59                 return -ENODEV;
  60
  61         pi->pairset[pairset_num].np = pairset_np;
  62
  63         return 0;
  64 }
  65
  66 /**
  67  * of_load_pse_pi_pairsets - load PSE PI pairsets pinout and polarity
  68  * @node: a pointer of the device node
  69  * @pi: a pointer of the PSE PI to fill
  70  * @npairsets: the number of pairsets (1 or 2) used by the PI
  71  *
  72  * Return: 0 on success and failure value on error
  73  */
  74 static int of_load_pse_pi_pairsets(struct device_node *node,
  75                                    struct pse_pi *pi,
  76                                    int npairsets)
  77 {
  78         int i, ret;
  79
  80         ret = of_property_count_strings(node, "pairset-names");
  81         if (ret != npairsets) {
  82                 pr_err("pse: amount of pairsets and pairset-names is not equal %d != %d (%pOF)\n",
  83                        npairsets, ret, node);
  84                 return -EINVAL;
  85         }
  86
  87         for (i = 0; i < npairsets; i++) {
  88                 ret = of_load_single_pse_pi_pairset(node, pi, i);
  89                 if (ret)
  90                         goto out;
  91         }
  92
  93         if (npairsets == 2 &&
  94             pi->pairset[0].pinout == pi->pairset[1].pinout) {
  95                 pr_err("pse: two PI pairsets can not have identical pinout (%pOF)",
  96                        node);
  97                 ret = -EINVAL;
  98         }
  99
 100 out:
 101         /* If an error appears, release all the pairset device node kref */
 102         if (ret) {
 103                 of_node_put(pi->pairset[0].np);
 104                 pi->pairset[0].np = NULL;
 105                 of_node_put(pi->pairset[1].np);
 106                 pi->pairset[1].np = NULL;
 107         }
 108
 109         return ret;
 110 }
 111
 112 static void pse_release_pis(struct pse_controller_dev *pcdev)
 113 {
 114         int i;
 115
 116         for (i = 0; i < pcdev->nr_lines; i++) {
 117                 of_node_put(pcdev->pi[i].pairset[0].np);
 118                 of_node_put(pcdev->pi[i].pairset[1].np);
 119                 of_node_put(pcdev->pi[i].np);
 120         }
 121         kfree(pcdev->pi);
 122 }
 123
 124 /**
 125  * of_load_pse_pis - load all the PSE PIs
 126  * @pcdev: a pointer to the PSE controller device
 127  *
 128  * Return: 0 on success and failure value on error
 129  */
 130 static int of_load_pse_pis(struct pse_controller_dev *pcdev)
 131 {
 132         struct device_node *np = pcdev->dev->of_node;
 133         struct device_node *node, *pis;
 134         int ret;
 135
 136         if (!np)
 137                 return -ENODEV;
 138
 139         pcdev->pi = kcalloc(pcdev->nr_lines, sizeof(*pcdev->pi), GFP_KERNEL);
 140         if (!pcdev->pi)
 141                 return -ENOMEM;
 142
 143         pis = of_get_child_by_name(np, "pse-pis");
 144         if (!pis) {
 145                 /* no description of PSE PIs */
 146                 pcdev->no_of_pse_pi = true;
 147                 return 0;
 148         }
 149
 150         for_each_child_of_node(pis, node) {
 151                 struct pse_pi pi = {0};
 152                 u32 id;
 153
 154                 if (!of_node_name_eq(node, "pse-pi"))
 155                         continue;
 156
 157                 ret = of_property_read_u32(node, "reg", &id);
 158                 if (ret) {
 159                         dev_err(pcdev->dev,
 160                                 "can't get reg property for node '%pOF'",
 161                                 node);
 162                         goto out;
 163                 }
 164
 165                 if (id >= pcdev->nr_lines) {
 166                         dev_err(pcdev->dev,
 167                                 "reg value (%u) is out of range (%u) (%pOF)\n",
 168                                 id, pcdev->nr_lines, node);
 169                         ret = -EINVAL;
 170                         goto out;
 171                 }
 172
 173                 if (pcdev->pi[id].np) {
 174                         dev_err(pcdev->dev,
 175                                 "other node with same reg value was already registered. %pOF : %pOF\n",
 176                                 pcdev->pi[id].np, node);
 177                         ret = -EINVAL;
 178                         goto out;
 179                 }
 180
 181                 ret = of_count_phandle_with_args(node, "pairsets", NULL);
 182                 /* npairsets is limited to value one or two */
 183                 if (ret == 1 || ret == 2) {
 184                         ret = of_load_pse_pi_pairsets(node, &pi, ret);
 185                         if (ret)
 186                                 goto out;
 187                 } else if (ret != ENOENT) {
 188                         dev_err(pcdev->dev,
 189                                 "error: wrong number of pairsets. Should be 1 or 2, got %d (%pOF)\n",
 190                                 ret, node);
 191                         ret = -EINVAL;
 192                         goto out;
 193                 }
 194
 195                 of_node_get(node);
 196                 pi.np = node;
 197                 memcpy(&pcdev->pi[id], &pi, sizeof(pi));
 198         }
 199
 200         of_node_put(pis);
 201         return 0;
 202
 203 out:
 204         pse_release_pis(pcdev);
 205         of_node_put(node);
 206         of_node_put(pis);
 207         return ret;
 208 }
 209
 210 static int pse_pi_is_enabled(struct regulator_dev *rdev)
 211 {
 212         struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
 213         const struct pse_controller_ops *ops;
 214         int id, ret;
 215
 216         ops = pcdev->ops;
 217         if (!ops->pi_is_enabled)
 218                 return -EOPNOTSUPP;
 219
 220         id = rdev_get_id(rdev);
 221         mutex_lock(&pcdev->lock);
 222         ret = ops->pi_is_enabled(pcdev, id);
 223         mutex_unlock(&pcdev->lock);
 224
 225         return ret;
 226 }
 227
 228 static int pse_pi_enable(struct regulator_dev *rdev)
 229 {
 230         struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
 231         const struct pse_controller_ops *ops;
 232         int id, ret;
 233
 234         ops = pcdev->ops;
 235         if (!ops->pi_enable)
 236                 return -EOPNOTSUPP;
 237
 238         id = rdev_get_id(rdev);
 239         mutex_lock(&pcdev->lock);
 240         ret = ops->pi_enable(pcdev, id);
 241         if (!ret)
 242                 pcdev->pi[id].admin_state_enabled = 1;
 243         mutex_unlock(&pcdev->lock);
 244
 245         return ret;
 246 }
 247
 248 static int pse_pi_disable(struct regulator_dev *rdev)
 249 {
 250         struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
 251         const struct pse_controller_ops *ops;
 252         int id, ret;
 253
 254         ops = pcdev->ops;
 255         if (!ops->pi_disable)
 256                 return -EOPNOTSUPP;
 257
 258         id = rdev_get_id(rdev);
 259         mutex_lock(&pcdev->lock);
 260         ret = ops->pi_disable(pcdev, id);
 261         if (!ret)
 262                 pcdev->pi[id].admin_state_enabled = 0;
 263         mutex_unlock(&pcdev->lock);
 264
 265         return ret;
 266 }
 267
 268 static int _pse_pi_get_voltage(struct regulator_dev *rdev)
 269 {
 270         struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
 271         const struct pse_controller_ops *ops;
 272         int id;
 273
 274         ops = pcdev->ops;
 275         if (!ops->pi_get_voltage)
 276                 return -EOPNOTSUPP;
 277
 278         id = rdev_get_id(rdev);
 279         return ops->pi_get_voltage(pcdev, id);
 280 }
 281
 282 static int pse_pi_get_voltage(struct regulator_dev *rdev)
 283 {
 284         struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
 285         int ret;
 286
 287         mutex_lock(&pcdev->lock);
 288         ret = _pse_pi_get_voltage(rdev);
 289         mutex_unlock(&pcdev->lock);
 290
 291         return ret;
 292 }
 293
 294 static int _pse_ethtool_get_status(struct pse_controller_dev *pcdev,
 295                                    int id,
 296                                    struct netlink_ext_ack *extack,
 297                                    struct pse_control_status *status);
 298
 299 static int pse_pi_get_current_limit(struct regulator_dev *rdev)
 300 {
 301         struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
 302         const struct pse_controller_ops *ops;
 303         struct netlink_ext_ack extack = {};
 304         struct pse_control_status st = {};
 305         int id, uV, ret;
 306         s64 tmp_64;
 307
 308         ops = pcdev->ops;
 309         id = rdev_get_id(rdev);
 310         mutex_lock(&pcdev->lock);
 311         if (ops->pi_get_current_limit) {
 312                 ret = ops->pi_get_current_limit(pcdev, id);
 313                 goto out;
 314         }
 315
 316         /* If pi_get_current_limit() callback not populated get voltage
 317          * from pi_get_voltage() and power limit from ethtool_get_status()
 318          *  to calculate current limit.
 319          */
 320         ret = _pse_pi_get_voltage(rdev);
 321         if (!ret) {
 322                 dev_err(pcdev->dev, "Voltage null\n");
 323                 ret = -ERANGE;
 324                 goto out;
 325         }
 326         if (ret < 0)
 327                 goto out;
 328         uV = ret;
 329
 330         ret = _pse_ethtool_get_status(pcdev, id, &extack, &st);
 331         if (ret)
 332                 goto out;
 333
 334         if (!st.c33_avail_pw_limit) {
 335                 ret = -ENODATA;
 336                 goto out;
 337         }
 338
 339         tmp_64 = st.c33_avail_pw_limit;
 340         tmp_64 *= 1000000000ull;
 341         /* uA = mW * 1000000000 / uV */
 342         ret = DIV_ROUND_CLOSEST_ULL(tmp_64, uV);
 343
 344 out:
 345         mutex_unlock(&pcdev->lock);
 346         return ret;
 347 }
 348
 349 static int pse_pi_set_current_limit(struct regulator_dev *rdev, int min_uA,
 350                                     int max_uA)
 351 {
 352         struct pse_controller_dev *pcdev = rdev_get_drvdata(rdev);
 353         const struct pse_controller_ops *ops;
 354         int id, ret;
 355
 356         ops = pcdev->ops;
 357         if (!ops->pi_set_current_limit)
 358                 return -EOPNOTSUPP;
 359
 360         id = rdev_get_id(rdev);
 361         mutex_lock(&pcdev->lock);
 362         ret = ops->pi_set_current_limit(pcdev, id, max_uA);
 363         mutex_unlock(&pcdev->lock);
 364
 365         return ret;
 366 }
 367
 368 static const struct regulator_ops pse_pi_ops = {
 369         .is_enabled = pse_pi_is_enabled,
 370         .enable = pse_pi_enable,
 371         .disable = pse_pi_disable,
 372         .get_voltage = pse_pi_get_voltage,
 373         .get_current_limit = pse_pi_get_current_limit,
 374         .set_current_limit = pse_pi_set_current_limit,
 375 };
 376
 377 static int
 378 devm_pse_pi_regulator_register(struct pse_controller_dev *pcdev,
 379                                char *name, int id)
 380 {
 381         struct regulator_init_data *rinit_data;
 382         struct regulator_config rconfig = {0};
 383         struct regulator_desc *rdesc;
 384         struct regulator_dev *rdev;
 385
 386         rinit_data = devm_kzalloc(pcdev->dev, sizeof(*rinit_data),
 387                                   GFP_KERNEL);
 388         if (!rinit_data)
 389                 return -ENOMEM;
 390
 391         rdesc = devm_kzalloc(pcdev->dev, sizeof(*rdesc), GFP_KERNEL);
 392         if (!rdesc)
 393                 return -ENOMEM;
 394
 395         /* Regulator descriptor id have to be the same as its associated
 396          * PSE PI id for the well functioning of the PSE controls.
 397          */
 398         rdesc->id = id;
 399         rdesc->name = name;
 400         rdesc->type = REGULATOR_VOLTAGE;
 401         rdesc->ops = &pse_pi_ops;
 402         rdesc->owner = pcdev->owner;
 403
 404         rinit_data->constraints.valid_ops_mask = REGULATOR_CHANGE_STATUS;
 405
 406         if (pcdev->ops->pi_set_current_limit) {
 407                 rinit_data->constraints.valid_ops_mask |=
 408                         REGULATOR_CHANGE_CURRENT;
 409                 rinit_data->constraints.max_uA = MAX_PI_CURRENT;
 410         }
 411
 412         rinit_data->supply_regulator = "vpwr";
 413
 414         rconfig.dev = pcdev->dev;
 415         rconfig.driver_data = pcdev;
 416         rconfig.init_data = rinit_data;
 417
 418         rdev = devm_regulator_register(pcdev->dev, rdesc, &rconfig);
 419         if (IS_ERR(rdev)) {
 420                 dev_err_probe(pcdev->dev, PTR_ERR(rdev),
 421                               "Failed to register regulator\n");
 422                 return PTR_ERR(rdev);
 423         }
 424
 425         pcdev->pi[id].rdev = rdev;
 426
 427         return 0;
 428 }
 429
 430 /**
 431  * pse_controller_register - register a PSE controller device
 432  * @pcdev: a pointer to the initialized PSE controller device
 433  *
 434  * Return: 0 on success and failure value on error
 435  */
 436 int pse_controller_register(struct pse_controller_dev *pcdev)
 437 {
 438         size_t reg_name_len;
 439         int ret, i;
 440
 441         mutex_init(&pcdev->lock);
 442         INIT_LIST_HEAD(&pcdev->pse_control_head);
 443
 444         if (!pcdev->nr_lines)
 445                 pcdev->nr_lines = 1;
 446
 447         ret = of_load_pse_pis(pcdev);
 448         if (ret)
 449                 return ret;
 450
 451         if (pcdev->ops->setup_pi_matrix) {
 452                 ret = pcdev->ops->setup_pi_matrix(pcdev);
 453                 if (ret)
 454                         return ret;
 455         }
 456
 457         /* Each regulator name len is pcdev dev name + 7 char +
 458          * int max digit number (10) + 1
 459          */
 460         reg_name_len = strlen(dev_name(pcdev->dev)) + 18;
 461
 462         /* Register PI regulators */
 463         for (i = 0; i < pcdev->nr_lines; i++) {
 464                 char *reg_name;
 465
 466                 /* Do not register regulator for PIs not described */
 467                 if (!pcdev->no_of_pse_pi && !pcdev->pi[i].np)
 468                         continue;
 469
 470                 reg_name = devm_kzalloc(pcdev->dev, reg_name_len, GFP_KERNEL);
 471                 if (!reg_name)
 472                         return -ENOMEM;
 473
 474                 snprintf(reg_name, reg_name_len, "pse-%s_pi%d",
 475                          dev_name(pcdev->dev), i);
 476
 477                 ret = devm_pse_pi_regulator_register(pcdev, reg_name, i);
 478                 if (ret)
 479                         return ret;
 480         }
 481
 482         mutex_lock(&pse_list_mutex);
 483         list_add(&pcdev->list, &pse_controller_list);
 484         mutex_unlock(&pse_list_mutex);
 485
 486         return 0;
 487 }
 488 EXPORT_SYMBOL_GPL(pse_controller_register);
 489
 490 /**
 491  * pse_controller_unregister - unregister a PSE controller device
 492  * @pcdev: a pointer to the PSE controller device
 493  */
 494 void pse_controller_unregister(struct pse_controller_dev *pcdev)
 495 {
 496         pse_release_pis(pcdev);
 497         mutex_lock(&pse_list_mutex);
 498         list_del(&pcdev->list);
 499         mutex_unlock(&pse_list_mutex);
 500 }
 501 EXPORT_SYMBOL_GPL(pse_controller_unregister);
 502
 503 static void devm_pse_controller_release(struct device *dev, void *res)
 504 {
 505         pse_controller_unregister(*(struct pse_controller_dev **)res);
 506 }
 507
 508 /**
 509  * devm_pse_controller_register - resource managed pse_controller_register()
 510  * @dev: device that is registering this PSE controller
 511  * @pcdev: a pointer to the initialized PSE controller device
 512  *
 513  * Managed pse_controller_register(). For PSE controllers registered by
 514  * this function, pse_controller_unregister() is automatically called on
 515  * driver detach. See pse_controller_register() for more information.
 516  *
 517  * Return: 0 on success and failure value on error
 518  */
 519 int devm_pse_controller_register(struct device *dev,
 520                                  struct pse_controller_dev *pcdev)
 521 {
 522         struct pse_controller_dev **pcdevp;
 523         int ret;
 524
 525         pcdevp = devres_alloc(devm_pse_controller_release, sizeof(*pcdevp),
 526                               GFP_KERNEL);
 527         if (!pcdevp)
 528                 return -ENOMEM;
 529
 530         ret = pse_controller_register(pcdev);
 531         if (ret) {
 532                 devres_free(pcdevp);
 533                 return ret;
 534         }
 535
 536         *pcdevp = pcdev;
 537         devres_add(dev, pcdevp);
 538
 539         return 0;
 540 }
 541 EXPORT_SYMBOL_GPL(devm_pse_controller_register);
 542
 543 /* PSE control section */
 544
 545 static void __pse_control_release(struct kref *kref)
 546 {
 547         struct pse_control *psec = container_of(kref, struct pse_control,
 548                                                   refcnt);
 549
 550         lockdep_assert_held(&pse_list_mutex);
 551
 552         if (psec->pcdev->pi[psec->id].admin_state_enabled)
 553                 regulator_disable(psec->ps);
 554         devm_regulator_put(psec->ps);
 555
 556         module_put(psec->pcdev->owner);
 557
 558         list_del(&psec->list);
 559         kfree(psec);
 560 }
 561
 562 static void __pse_control_put_internal(struct pse_control *psec)
 563 {
 564         lockdep_assert_held(&pse_list_mutex);
 565
 566         kref_put(&psec->refcnt, __pse_control_release);
 567 }
 568
 569 /**
 570  * pse_control_put - free the PSE control
 571  * @psec: PSE control pointer
 572  */
 573 void pse_control_put(struct pse_control *psec)
 574 {
 575         if (IS_ERR_OR_NULL(psec))
 576                 return;
 577
 578         mutex_lock(&pse_list_mutex);
 579         __pse_control_put_internal(psec);
 580         mutex_unlock(&pse_list_mutex);
 581 }
 582 EXPORT_SYMBOL_GPL(pse_control_put);
 583
 584 static struct pse_control *
 585 pse_control_get_internal(struct pse_controller_dev *pcdev, unsigned int index)
 586 {
 587         struct pse_control *psec;
 588         int ret;
 589
 590         lockdep_assert_held(&pse_list_mutex);
 591
 592         list_for_each_entry(psec, &pcdev->pse_control_head, list) {
 593                 if (psec->id == index) {
 594                         kref_get(&psec->refcnt);
 595                         return psec;
 596                 }
 597         }
 598
 599         psec = kzalloc(sizeof(*psec), GFP_KERNEL);
 600         if (!psec)
 601                 return ERR_PTR(-ENOMEM);
 602
 603         if (!try_module_get(pcdev->owner)) {
 604                 ret = -ENODEV;
 605                 goto free_psec;
 606         }
 607
 608         psec->ps = devm_regulator_get_exclusive(pcdev->dev,
 609                                                 rdev_get_name(pcdev->pi[index].rdev));
 610         if (IS_ERR(psec->ps)) {
 611                 ret = PTR_ERR(psec->ps);
 612                 goto put_module;
 613         }
 614
 615         ret = regulator_is_enabled(psec->ps);
 616         if (ret < 0)
 617                 goto regulator_put;
 618
 619         pcdev->pi[index].admin_state_enabled = ret;
 620
 621         psec->pcdev = pcdev;
 622         list_add(&psec->list, &pcdev->pse_control_head);
 623         psec->id = index;
 624         kref_init(&psec->refcnt);
 625
 626         return psec;
 627
 628 regulator_put:
 629         devm_regulator_put(psec->ps);
 630 put_module:
 631         module_put(pcdev->owner);
 632 free_psec:
 633         kfree(psec);
 634
 635         return ERR_PTR(ret);
 636 }
 637
 638 /**
 639  * of_pse_match_pi - Find the PSE PI id matching the device node phandle
 640  * @pcdev: a pointer to the PSE controller device
 641  * @np: a pointer to the device node
 642  *
 643  * Return: id of the PSE PI, -EINVAL if not found
 644  */
 645 static int of_pse_match_pi(struct pse_controller_dev *pcdev,
 646                            struct device_node *np)
 647 {
 648         int i;
 649
 650         for (i = 0; i < pcdev->nr_lines; i++) {
 651                 if (pcdev->pi[i].np == np)
 652                         return i;
 653         }
 654
 655         return -EINVAL;
 656 }
 657
 658 /**
 659  * psec_id_xlate - translate pse_spec to the PSE line number according
 660  *                 to the number of pse-cells in case of no pse_pi node
 661  * @pcdev: a pointer to the PSE controller device
 662  * @pse_spec: PSE line specifier as found in the device tree
 663  *
 664  * Return: 0 if #pse-cells = <0>. Return PSE line number otherwise.
 665  */
 666 static int psec_id_xlate(struct pse_controller_dev *pcdev,
 667                          const struct of_phandle_args *pse_spec)
 668 {
 669         if (!pcdev->of_pse_n_cells)
 670                 return 0;
 671
 672         if (pcdev->of_pse_n_cells > 1 ||
 673             pse_spec->args[0] >= pcdev->nr_lines)
 674                 return -EINVAL;
 675
 676         return pse_spec->args[0];
 677 }
 678
 679 struct pse_control *of_pse_control_get(struct device_node *node)
 680 {
 681         struct pse_controller_dev *r, *pcdev;
 682         struct of_phandle_args args;
 683         struct pse_control *psec;
 684         int psec_id;
 685         int ret;
 686
 687         if (!node)
 688                 return ERR_PTR(-EINVAL);
 689
 690         ret = of_parse_phandle_with_args(node, "pses", "#pse-cells", 0, &args);
 691         if (ret)
 692                 return ERR_PTR(ret);
 693
 694         mutex_lock(&pse_list_mutex);
 695         pcdev = NULL;
 696         list_for_each_entry(r, &pse_controller_list, list) {
 697                 if (!r->no_of_pse_pi) {
 698                         ret = of_pse_match_pi(r, args.np);
 699                         if (ret >= 0) {
 700                                 pcdev = r;
 701                                 psec_id = ret;
 702                                 break;
 703                         }
 704                 } else if (args.np == r->dev->of_node) {
 705                         pcdev = r;
 706                         break;
 707                 }
 708         }
 709
 710         if (!pcdev) {
 711                 psec = ERR_PTR(-EPROBE_DEFER);
 712                 goto out;
 713         }
 714
 715         if (WARN_ON(args.args_count != pcdev->of_pse_n_cells)) {
 716                 psec = ERR_PTR(-EINVAL);
 717                 goto out;
 718         }
 719
 720         if (pcdev->no_of_pse_pi) {
 721                 psec_id = psec_id_xlate(pcdev, &args);
 722                 if (psec_id < 0) {
 723                         psec = ERR_PTR(psec_id);
 724                         goto out;
 725                 }
 726         }
 727
 728         /* pse_list_mutex also protects the pcdev's pse_control list */
 729         psec = pse_control_get_internal(pcdev, psec_id);
 730
 731 out:
 732         mutex_unlock(&pse_list_mutex);
 733         of_node_put(args.np);
 734
 735         return psec;
 736 }
 737 EXPORT_SYMBOL_GPL(of_pse_control_get);
 738
 739 static int _pse_ethtool_get_status(struct pse_controller_dev *pcdev,
 740                                    int id,
 741                                    struct netlink_ext_ack *extack,
 742                                    struct pse_control_status *status)
 743 {
 744         const struct pse_controller_ops *ops;
 745
 746         ops = pcdev->ops;
 747         if (!ops->ethtool_get_status) {
 748                 NL_SET_ERR_MSG(extack,
 749                                "PSE driver does not support status report");
 750                 return -EOPNOTSUPP;
 751         }
 752
 753         return ops->ethtool_get_status(pcdev, id, extack, status);
 754 }
 755
 756 /**
 757  * pse_ethtool_get_status - get status of PSE control
 758  * @psec: PSE control pointer
 759  * @extack: extack for reporting useful error messages
 760  * @status: struct to store PSE status
 761  *
 762  * Return: 0 on success and failure value on error
 763  */
 764 int pse_ethtool_get_status(struct pse_control *psec,
 765                            struct netlink_ext_ack *extack,
 766                            struct pse_control_status *status)
 767 {
 768         int err;
 769
 770         mutex_lock(&psec->pcdev->lock);
 771         err = _pse_ethtool_get_status(psec->pcdev, psec->id, extack, status);
 772         mutex_unlock(&psec->pcdev->lock);
 773
 774         return err;
 775 }
 776 EXPORT_SYMBOL_GPL(pse_ethtool_get_status);
 777
 778 static int pse_ethtool_c33_set_config(struct pse_control *psec,
 779                                       const struct pse_control_config *config)
 780 {
 781         int err = 0;
 782
 783         /* Look at admin_state_enabled status to not call regulator_enable
 784          * or regulator_disable twice creating a regulator counter mismatch
 785          */
 786         switch (config->c33_admin_control) {
 787         case ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED:
 788                 /* We could have mismatch between admin_state_enabled and
 789                  * state reported by regulator_is_enabled. This can occur when
 790                  * the PI is forcibly turn off by the controller. Call
 791                  * regulator_disable on that case to fix the counters state.
 792                  */
 793                 if (psec->pcdev->pi[psec->id].admin_state_enabled &&
 794                     !regulator_is_enabled(psec->ps)) {
 795                         err = regulator_disable(psec->ps);
 796                         if (err)
 797                                 break;
 798                 }
 799                 if (!psec->pcdev->pi[psec->id].admin_state_enabled)
 800                         err = regulator_enable(psec->ps);
 801                 break;
 802         case ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED:
 803                 if (psec->pcdev->pi[psec->id].admin_state_enabled)
 804                         err = regulator_disable(psec->ps);
 805                 break;
 806         default:
 807                 err = -EOPNOTSUPP;
 808         }
 809
 810         return err;
 811 }
 812
 813 static int pse_ethtool_podl_set_config(struct pse_control *psec,
 814                                        const struct pse_control_config *config)
 815 {
 816         int err = 0;
 817
 818         /* Look at admin_state_enabled status to not call regulator_enable
 819          * or regulator_disable twice creating a regulator counter mismatch
 820          */
 821         switch (config->podl_admin_control) {
 822         case ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED:
 823                 if (!psec->pcdev->pi[psec->id].admin_state_enabled)
 824                         err = regulator_enable(psec->ps);
 825                 break;
 826         case ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED:
 827                 if (psec->pcdev->pi[psec->id].admin_state_enabled)
 828                         err = regulator_disable(psec->ps);
 829                 break;
 830         default:
 831                 err = -EOPNOTSUPP;
 832         }
 833
 834         return err;
 835 }
 836
 837 /**
 838  * pse_ethtool_set_config - set PSE control configuration
 839  * @psec: PSE control pointer
 840  * @extack: extack for reporting useful error messages
 841  * @config: Configuration of the test to run
 842  *
 843  * Return: 0 on success and failure value on error
 844  */
 845 int pse_ethtool_set_config(struct pse_control *psec,
 846                            struct netlink_ext_ack *extack,
 847                            const struct pse_control_config *config)
 848 {
 849         int err = 0;
 850
 851         if (pse_has_c33(psec) && config->c33_admin_control) {
 852                 err = pse_ethtool_c33_set_config(psec, config);
 853                 if (err)
 854                         return err;
 855         }
 856
 857         if (pse_has_podl(psec) && config->podl_admin_control)
 858                 err = pse_ethtool_podl_set_config(psec, config);
 859
 860         return err;
 861 }
 862 EXPORT_SYMBOL_GPL(pse_ethtool_set_config);
 863
 864 /**
 865  * pse_ethtool_set_pw_limit - set PSE control power limit
 866  * @psec: PSE control pointer
 867  * @extack: extack for reporting useful error messages
 868  * @pw_limit: power limit value in mW
 869  *
 870  * Return: 0 on success and failure value on error
 871  */
 872 int pse_ethtool_set_pw_limit(struct pse_control *psec,
 873                              struct netlink_ext_ack *extack,
 874                              const unsigned int pw_limit)
 875 {
 876         int uV, uA, ret;
 877         s64 tmp_64;
 878
 879         ret = regulator_get_voltage(psec->ps);
 880         if (!ret) {
 881                 NL_SET_ERR_MSG(extack,
 882                                "Can't calculate the current, PSE voltage read is 0");
 883                 return -ERANGE;
 884         }
 885         if (ret < 0) {
 886                 NL_SET_ERR_MSG(extack,
 887                                "Error reading PSE voltage");
 888                 return ret;
 889         }
 890         uV = ret;
 891
 892         tmp_64 = pw_limit;
 893         tmp_64 *= 1000000000ull;
 894         /* uA = mW * 1000000000 / uV */
 895         uA = DIV_ROUND_CLOSEST_ULL(tmp_64, uV);
 896
 897         return regulator_set_current_limit(psec->ps, 0, uA);
 898 }
 899 EXPORT_SYMBOL_GPL(pse_ethtool_set_pw_limit);
 900
 901 bool pse_has_podl(struct pse_control *psec)
 902 {
 903         return psec->pcdev->types & ETHTOOL_PSE_PODL;
 904 }
 905 EXPORT_SYMBOL_GPL(pse_has_podl);
 906
 907 bool pse_has_c33(struct pse_control *psec)
 908 {
 909         return psec->pcdev->types & ETHTOOL_PSE_C33;
 910 }
 911 EXPORT_SYMBOL_GPL(pse_has_c33);