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 <[email protected]>
   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                                                  REGULATOR_CHANGE_CURRENT;
 406         rinit_data->constraints.max_uA = MAX_PI_CURRENT;
 407         rinit_data->supply_regulator = "vpwr";
 408
 409         rconfig.dev = pcdev->dev;
 410         rconfig.driver_data = pcdev;
 411         rconfig.init_data = rinit_data;
 412
 413         rdev = devm_regulator_register(pcdev->dev, rdesc, &rconfig);
 414         if (IS_ERR(rdev)) {
 415                 dev_err_probe(pcdev->dev, PTR_ERR(rdev),
 416                               "Failed to register regulator\n");
 417                 return PTR_ERR(rdev);
 418         }
 419
 420         pcdev->pi[id].rdev = rdev;
 421
 422         return 0;
 423 }
 424
 425 /**
 426  * pse_controller_register - register a PSE controller device
 427  * @pcdev: a pointer to the initialized PSE controller device
 428  *
 429  * Return: 0 on success and failure value on error
 430  */
 431 int pse_controller_register(struct pse_controller_dev *pcdev)
 432 {
 433         size_t reg_name_len;
 434         int ret, i;
 435
 436         mutex_init(&pcdev->lock);
 437         INIT_LIST_HEAD(&pcdev->pse_control_head);
 438
 439         if (!pcdev->nr_lines)
 440                 pcdev->nr_lines = 1;
 441
 442         ret = of_load_pse_pis(pcdev);
 443         if (ret)
 444                 return ret;
 445
 446         if (pcdev->ops->setup_pi_matrix) {
 447                 ret = pcdev->ops->setup_pi_matrix(pcdev);
 448                 if (ret)
 449                         return ret;
 450         }
 451
 452         /* Each regulator name len is pcdev dev name + 7 char +
 453          * int max digit number (10) + 1
 454          */
 455         reg_name_len = strlen(dev_name(pcdev->dev)) + 18;
 456
 457         /* Register PI regulators */
 458         for (i = 0; i < pcdev->nr_lines; i++) {
 459                 char *reg_name;
 460
 461                 /* Do not register regulator for PIs not described */
 462                 if (!pcdev->no_of_pse_pi && !pcdev->pi[i].np)
 463                         continue;
 464
 465                 reg_name = devm_kzalloc(pcdev->dev, reg_name_len, GFP_KERNEL);
 466                 if (!reg_name)
 467                         return -ENOMEM;
 468
 469                 snprintf(reg_name, reg_name_len, "pse-%s_pi%d",
 470                          dev_name(pcdev->dev), i);
 471
 472                 ret = devm_pse_pi_regulator_register(pcdev, reg_name, i);
 473                 if (ret)
 474                         return ret;
 475         }
 476
 477         mutex_lock(&pse_list_mutex);
 478         list_add(&pcdev->list, &pse_controller_list);
 479         mutex_unlock(&pse_list_mutex);
 480
 481         return 0;
 482 }
 483 EXPORT_SYMBOL_GPL(pse_controller_register);
 484
 485 /**
 486  * pse_controller_unregister - unregister a PSE controller device
 487  * @pcdev: a pointer to the PSE controller device
 488  */
 489 void pse_controller_unregister(struct pse_controller_dev *pcdev)
 490 {
 491         pse_release_pis(pcdev);
 492         mutex_lock(&pse_list_mutex);
 493         list_del(&pcdev->list);
 494         mutex_unlock(&pse_list_mutex);
 495 }
 496 EXPORT_SYMBOL_GPL(pse_controller_unregister);
 497
 498 static void devm_pse_controller_release(struct device *dev, void *res)
 499 {
 500         pse_controller_unregister(*(struct pse_controller_dev **)res);
 501 }
 502
 503 /**
 504  * devm_pse_controller_register - resource managed pse_controller_register()
 505  * @dev: device that is registering this PSE controller
 506  * @pcdev: a pointer to the initialized PSE controller device
 507  *
 508  * Managed pse_controller_register(). For PSE controllers registered by
 509  * this function, pse_controller_unregister() is automatically called on
 510  * driver detach. See pse_controller_register() for more information.
 511  *
 512  * Return: 0 on success and failure value on error
 513  */
 514 int devm_pse_controller_register(struct device *dev,
 515                                  struct pse_controller_dev *pcdev)
 516 {
 517         struct pse_controller_dev **pcdevp;
 518         int ret;
 519
 520         pcdevp = devres_alloc(devm_pse_controller_release, sizeof(*pcdevp),
 521                               GFP_KERNEL);
 522         if (!pcdevp)
 523                 return -ENOMEM;
 524
 525         ret = pse_controller_register(pcdev);
 526         if (ret) {
 527                 devres_free(pcdevp);
 528                 return ret;
 529         }
 530
 531         *pcdevp = pcdev;
 532         devres_add(dev, pcdevp);
 533
 534         return 0;
 535 }
 536 EXPORT_SYMBOL_GPL(devm_pse_controller_register);
 537
 538 /* PSE control section */
 539
 540 static void __pse_control_release(struct kref *kref)
 541 {
 542         struct pse_control *psec = container_of(kref, struct pse_control,
 543                                                   refcnt);
 544
 545         lockdep_assert_held(&pse_list_mutex);
 546
 547         if (psec->pcdev->pi[psec->id].admin_state_enabled)
 548                 regulator_disable(psec->ps);
 549         devm_regulator_put(psec->ps);
 550
 551         module_put(psec->pcdev->owner);
 552
 553         list_del(&psec->list);
 554         kfree(psec);
 555 }
 556
 557 static void __pse_control_put_internal(struct pse_control *psec)
 558 {
 559         lockdep_assert_held(&pse_list_mutex);
 560
 561         kref_put(&psec->refcnt, __pse_control_release);
 562 }
 563
 564 /**
 565  * pse_control_put - free the PSE control
 566  * @psec: PSE control pointer
 567  */
 568 void pse_control_put(struct pse_control *psec)
 569 {
 570         if (IS_ERR_OR_NULL(psec))
 571                 return;
 572
 573         mutex_lock(&pse_list_mutex);
 574         __pse_control_put_internal(psec);
 575         mutex_unlock(&pse_list_mutex);
 576 }
 577 EXPORT_SYMBOL_GPL(pse_control_put);
 578
 579 static struct pse_control *
 580 pse_control_get_internal(struct pse_controller_dev *pcdev, unsigned int index)
 581 {
 582         struct pse_control *psec;
 583         int ret;
 584
 585         lockdep_assert_held(&pse_list_mutex);
 586
 587         list_for_each_entry(psec, &pcdev->pse_control_head, list) {
 588                 if (psec->id == index) {
 589                         kref_get(&psec->refcnt);
 590                         return psec;
 591                 }
 592         }
 593
 594         psec = kzalloc(sizeof(*psec), GFP_KERNEL);
 595         if (!psec)
 596                 return ERR_PTR(-ENOMEM);
 597
 598         if (!try_module_get(pcdev->owner)) {
 599                 ret = -ENODEV;
 600                 goto free_psec;
 601         }
 602
 603         psec->ps = devm_regulator_get_exclusive(pcdev->dev,
 604                                                 rdev_get_name(pcdev->pi[index].rdev));
 605         if (IS_ERR(psec->ps)) {
 606                 ret = PTR_ERR(psec->ps);
 607                 goto put_module;
 608         }
 609
 610         ret = regulator_is_enabled(psec->ps);
 611         if (ret < 0)
 612                 goto regulator_put;
 613
 614         pcdev->pi[index].admin_state_enabled = ret;
 615
 616         psec->pcdev = pcdev;
 617         list_add(&psec->list, &pcdev->pse_control_head);
 618         psec->id = index;
 619         kref_init(&psec->refcnt);
 620
 621         return psec;
 622
 623 regulator_put:
 624         devm_regulator_put(psec->ps);
 625 put_module:
 626         module_put(pcdev->owner);
 627 free_psec:
 628         kfree(psec);
 629
 630         return ERR_PTR(ret);
 631 }
 632
 633 /**
 634  * of_pse_match_pi - Find the PSE PI id matching the device node phandle
 635  * @pcdev: a pointer to the PSE controller device
 636  * @np: a pointer to the device node
 637  *
 638  * Return: id of the PSE PI, -EINVAL if not found
 639  */
 640 static int of_pse_match_pi(struct pse_controller_dev *pcdev,
 641                            struct device_node *np)
 642 {
 643         int i;
 644
 645         for (i = 0; i <= pcdev->nr_lines; i++) {
 646                 if (pcdev->pi[i].np == np)
 647                         return i;
 648         }
 649
 650         return -EINVAL;
 651 }
 652
 653 /**
 654  * psec_id_xlate - translate pse_spec to the PSE line number according
 655  *                 to the number of pse-cells in case of no pse_pi node
 656  * @pcdev: a pointer to the PSE controller device
 657  * @pse_spec: PSE line specifier as found in the device tree
 658  *
 659  * Return: 0 if #pse-cells = <0>. Return PSE line number otherwise.
 660  */
 661 static int psec_id_xlate(struct pse_controller_dev *pcdev,
 662                          const struct of_phandle_args *pse_spec)
 663 {
 664         if (!pcdev->of_pse_n_cells)
 665                 return 0;
 666
 667         if (pcdev->of_pse_n_cells > 1 ||
 668             pse_spec->args[0] >= pcdev->nr_lines)
 669                 return -EINVAL;
 670
 671         return pse_spec->args[0];
 672 }
 673
 674 struct pse_control *of_pse_control_get(struct device_node *node)
 675 {
 676         struct pse_controller_dev *r, *pcdev;
 677         struct of_phandle_args args;
 678         struct pse_control *psec;
 679         int psec_id;
 680         int ret;
 681
 682         if (!node)
 683                 return ERR_PTR(-EINVAL);
 684
 685         ret = of_parse_phandle_with_args(node, "pses", "#pse-cells", 0, &args);
 686         if (ret)
 687                 return ERR_PTR(ret);
 688
 689         mutex_lock(&pse_list_mutex);
 690         pcdev = NULL;
 691         list_for_each_entry(r, &pse_controller_list, list) {
 692                 if (!r->no_of_pse_pi) {
 693                         ret = of_pse_match_pi(r, args.np);
 694                         if (ret >= 0) {
 695                                 pcdev = r;
 696                                 psec_id = ret;
 697                                 break;
 698                         }
 699                 } else if (args.np == r->dev->of_node) {
 700                         pcdev = r;
 701                         break;
 702                 }
 703         }
 704
 705         if (!pcdev) {
 706                 psec = ERR_PTR(-EPROBE_DEFER);
 707                 goto out;
 708         }
 709
 710         if (WARN_ON(args.args_count != pcdev->of_pse_n_cells)) {
 711                 psec = ERR_PTR(-EINVAL);
 712                 goto out;
 713         }
 714
 715         if (pcdev->no_of_pse_pi) {
 716                 psec_id = psec_id_xlate(pcdev, &args);
 717                 if (psec_id < 0) {
 718                         psec = ERR_PTR(psec_id);
 719                         goto out;
 720                 }
 721         }
 722
 723         /* pse_list_mutex also protects the pcdev's pse_control list */
 724         psec = pse_control_get_internal(pcdev, psec_id);
 725
 726 out:
 727         mutex_unlock(&pse_list_mutex);
 728         of_node_put(args.np);
 729
 730         return psec;
 731 }
 732 EXPORT_SYMBOL_GPL(of_pse_control_get);
 733
 734 static int _pse_ethtool_get_status(struct pse_controller_dev *pcdev,
 735                                    int id,
 736                                    struct netlink_ext_ack *extack,
 737                                    struct pse_control_status *status)
 738 {
 739         const struct pse_controller_ops *ops;
 740
 741         ops = pcdev->ops;
 742         if (!ops->ethtool_get_status) {
 743                 NL_SET_ERR_MSG(extack,
 744                                "PSE driver does not support status report");
 745                 return -EOPNOTSUPP;
 746         }
 747
 748         return ops->ethtool_get_status(pcdev, id, extack, status);
 749 }
 750
 751 /**
 752  * pse_ethtool_get_status - get status of PSE control
 753  * @psec: PSE control pointer
 754  * @extack: extack for reporting useful error messages
 755  * @status: struct to store PSE status
 756  *
 757  * Return: 0 on success and failure value on error
 758  */
 759 int pse_ethtool_get_status(struct pse_control *psec,
 760                            struct netlink_ext_ack *extack,
 761                            struct pse_control_status *status)
 762 {
 763         int err;
 764
 765         mutex_lock(&psec->pcdev->lock);
 766         err = _pse_ethtool_get_status(psec->pcdev, psec->id, extack, status);
 767         mutex_unlock(&psec->pcdev->lock);
 768
 769         return err;
 770 }
 771 EXPORT_SYMBOL_GPL(pse_ethtool_get_status);
 772
 773 static int pse_ethtool_c33_set_config(struct pse_control *psec,
 774                                       const struct pse_control_config *config)
 775 {
 776         int err = 0;
 777
 778         /* Look at admin_state_enabled status to not call regulator_enable
 779          * or regulator_disable twice creating a regulator counter mismatch
 780          */
 781         switch (config->c33_admin_control) {
 782         case ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED:
 783                 if (!psec->pcdev->pi[psec->id].admin_state_enabled)
 784                         err = regulator_enable(psec->ps);
 785                 break;
 786         case ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED:
 787                 if (psec->pcdev->pi[psec->id].admin_state_enabled)
 788                         err = regulator_disable(psec->ps);
 789                 break;
 790         default:
 791                 err = -EOPNOTSUPP;
 792         }
 793
 794         return err;
 795 }
 796
 797 static int pse_ethtool_podl_set_config(struct pse_control *psec,
 798                                        const struct pse_control_config *config)
 799 {
 800         int err = 0;
 801
 802         /* Look at admin_state_enabled status to not call regulator_enable
 803          * or regulator_disable twice creating a regulator counter mismatch
 804          */
 805         switch (config->podl_admin_control) {
 806         case ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED:
 807                 if (!psec->pcdev->pi[psec->id].admin_state_enabled)
 808                         err = regulator_enable(psec->ps);
 809                 break;
 810         case ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED:
 811                 if (psec->pcdev->pi[psec->id].admin_state_enabled)
 812                         err = regulator_disable(psec->ps);
 813                 break;
 814         default:
 815                 err = -EOPNOTSUPP;
 816         }
 817
 818         return err;
 819 }
 820
 821 /**
 822  * pse_ethtool_set_config - set PSE control configuration
 823  * @psec: PSE control pointer
 824  * @extack: extack for reporting useful error messages
 825  * @config: Configuration of the test to run
 826  *
 827  * Return: 0 on success and failure value on error
 828  */
 829 int pse_ethtool_set_config(struct pse_control *psec,
 830                            struct netlink_ext_ack *extack,
 831                            const struct pse_control_config *config)
 832 {
 833         int err = 0;
 834
 835         if (pse_has_c33(psec) && config->c33_admin_control) {
 836                 err = pse_ethtool_c33_set_config(psec, config);
 837                 if (err)
 838                         return err;
 839         }
 840
 841         if (pse_has_podl(psec) && config->podl_admin_control)
 842                 err = pse_ethtool_podl_set_config(psec, config);
 843
 844         return err;
 845 }
 846 EXPORT_SYMBOL_GPL(pse_ethtool_set_config);
 847
 848 /**
 849  * pse_ethtool_set_pw_limit - set PSE control power limit
 850  * @psec: PSE control pointer
 851  * @extack: extack for reporting useful error messages
 852  * @pw_limit: power limit value in mW
 853  *
 854  * Return: 0 on success and failure value on error
 855  */
 856 int pse_ethtool_set_pw_limit(struct pse_control *psec,
 857                              struct netlink_ext_ack *extack,
 858                              const unsigned int pw_limit)
 859 {
 860         int uV, uA, ret;
 861         s64 tmp_64;
 862
 863         ret = regulator_get_voltage(psec->ps);
 864         if (!ret) {
 865                 NL_SET_ERR_MSG(extack,
 866                                "Can't calculate the current, PSE voltage read is 0");
 867                 return -ERANGE;
 868         }
 869         if (ret < 0) {
 870                 NL_SET_ERR_MSG(extack,
 871                                "Error reading PSE voltage");
 872                 return ret;
 873         }
 874         uV = ret;
 875
 876         tmp_64 = pw_limit;
 877         tmp_64 *= 1000000000ull;
 878         /* uA = mW * 1000000000 / uV */
 879         uA = DIV_ROUND_CLOSEST_ULL(tmp_64, uV);
 880
 881         return regulator_set_current_limit(psec->ps, 0, uA);
 882 }
 883 EXPORT_SYMBOL_GPL(pse_ethtool_set_pw_limit);
 884
 885 bool pse_has_podl(struct pse_control *psec)
 886 {
 887         return psec->pcdev->types & ETHTOOL_PSE_PODL;
 888 }
 889 EXPORT_SYMBOL_GPL(pse_has_podl);
 890
 891 bool pse_has_c33(struct pse_control *psec)
 892 {
 893         return psec->pcdev->types & ETHTOOL_PSE_C33;
 894 }
 895 EXPORT_SYMBOL_GPL(pse_has_c33);