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
  12 static DEFINE_MUTEX(pse_list_mutex);
  13 static LIST_HEAD(pse_controller_list);
  14
  15 /**
  16  * struct pse_control - a PSE control
  17  * @pcdev: a pointer to the PSE controller device
  18  *         this PSE control belongs to
  19  * @list: list entry for the pcdev's PSE controller list
  20  * @id: ID of the PSE line in the PSE controller device
  21  * @refcnt: Number of gets of this pse_control
  22  */
  23 struct pse_control {
  24         struct pse_controller_dev *pcdev;
  25         struct list_head list;
  26         unsigned int id;
  27         struct kref refcnt;
  28 };
  29
  30 /**
  31  * of_pse_zero_xlate - dummy function for controllers with one only control
  32  * @pcdev: a pointer to the PSE controller device
  33  * @pse_spec: PSE line specifier as found in the device tree
  34  *
  35  * This static translation function is used by default if of_xlate in
  36  * :c:type:`pse_controller_dev` is not set. It is useful for all PSE
  37  * controllers with #pse-cells = <0>.
  38  */
  39 static int of_pse_zero_xlate(struct pse_controller_dev *pcdev,
  40                              const struct of_phandle_args *pse_spec)
  41 {
  42         return 0;
  43 }
  44
  45 /**
  46  * of_pse_simple_xlate - translate pse_spec to the PSE line number
  47  * @pcdev: a pointer to the PSE controller device
  48  * @pse_spec: PSE line specifier as found in the device tree
  49  *
  50  * This static translation function is used by default if of_xlate in
  51  * :c:type:`pse_controller_dev` is not set. It is useful for all PSE
  52  * controllers with 1:1 mapping, where PSE lines can be indexed by number
  53  * without gaps.
  54  */
  55 static int of_pse_simple_xlate(struct pse_controller_dev *pcdev,
  56                                const struct of_phandle_args *pse_spec)
  57 {
  58         if (pse_spec->args[0] >= pcdev->nr_lines)
  59                 return -EINVAL;
  60
  61         return pse_spec->args[0];
  62 }
  63
  64 /**
  65  * pse_controller_register - register a PSE controller device
  66  * @pcdev: a pointer to the initialized PSE controller device
  67  */
  68 int pse_controller_register(struct pse_controller_dev *pcdev)
  69 {
  70         if (!pcdev->of_xlate) {
  71                 if (pcdev->of_pse_n_cells == 0)
  72                         pcdev->of_xlate = of_pse_zero_xlate;
  73                 else if (pcdev->of_pse_n_cells == 1)
  74                         pcdev->of_xlate = of_pse_simple_xlate;
  75         }
  76
  77         mutex_init(&pcdev->lock);
  78         INIT_LIST_HEAD(&pcdev->pse_control_head);
  79
  80         mutex_lock(&pse_list_mutex);
  81         list_add(&pcdev->list, &pse_controller_list);
  82         mutex_unlock(&pse_list_mutex);
  83
  84         return 0;
  85 }
  86 EXPORT_SYMBOL_GPL(pse_controller_register);
  87
  88 /**
  89  * pse_controller_unregister - unregister a PSE controller device
  90  * @pcdev: a pointer to the PSE controller device
  91  */
  92 void pse_controller_unregister(struct pse_controller_dev *pcdev)
  93 {
  94         mutex_lock(&pse_list_mutex);
  95         list_del(&pcdev->list);
  96         mutex_unlock(&pse_list_mutex);
  97 }
  98 EXPORT_SYMBOL_GPL(pse_controller_unregister);
  99
 100 static void devm_pse_controller_release(struct device *dev, void *res)
 101 {
 102         pse_controller_unregister(*(struct pse_controller_dev **)res);
 103 }
 104
 105 /**
 106  * devm_pse_controller_register - resource managed pse_controller_register()
 107  * @dev: device that is registering this PSE controller
 108  * @pcdev: a pointer to the initialized PSE controller device
 109  *
 110  * Managed pse_controller_register(). For PSE controllers registered by
 111  * this function, pse_controller_unregister() is automatically called on
 112  * driver detach. See pse_controller_register() for more information.
 113  */
 114 int devm_pse_controller_register(struct device *dev,
 115                                  struct pse_controller_dev *pcdev)
 116 {
 117         struct pse_controller_dev **pcdevp;
 118         int ret;
 119
 120         pcdevp = devres_alloc(devm_pse_controller_release, sizeof(*pcdevp),
 121                               GFP_KERNEL);
 122         if (!pcdevp)
 123                 return -ENOMEM;
 124
 125         ret = pse_controller_register(pcdev);
 126         if (ret) {
 127                 devres_free(pcdevp);
 128                 return ret;
 129         }
 130
 131         *pcdevp = pcdev;
 132         devres_add(dev, pcdevp);
 133
 134         return 0;
 135 }
 136 EXPORT_SYMBOL_GPL(devm_pse_controller_register);
 137
 138 /* PSE control section */
 139
 140 static void __pse_control_release(struct kref *kref)
 141 {
 142         struct pse_control *psec = container_of(kref, struct pse_control,
 143                                                   refcnt);
 144
 145         lockdep_assert_held(&pse_list_mutex);
 146
 147         module_put(psec->pcdev->owner);
 148
 149         list_del(&psec->list);
 150         kfree(psec);
 151 }
 152
 153 static void __pse_control_put_internal(struct pse_control *psec)
 154 {
 155         lockdep_assert_held(&pse_list_mutex);
 156
 157         kref_put(&psec->refcnt, __pse_control_release);
 158 }
 159
 160 /**
 161  * pse_control_put - free the PSE control
 162  * @psec: PSE control pointer
 163  */
 164 void pse_control_put(struct pse_control *psec)
 165 {
 166         if (IS_ERR_OR_NULL(psec))
 167                 return;
 168
 169         mutex_lock(&pse_list_mutex);
 170         __pse_control_put_internal(psec);
 171         mutex_unlock(&pse_list_mutex);
 172 }
 173 EXPORT_SYMBOL_GPL(pse_control_put);
 174
 175 static struct pse_control *
 176 pse_control_get_internal(struct pse_controller_dev *pcdev, unsigned int index)
 177 {
 178         struct pse_control *psec;
 179
 180         lockdep_assert_held(&pse_list_mutex);
 181
 182         list_for_each_entry(psec, &pcdev->pse_control_head, list) {
 183                 if (psec->id == index) {
 184                         kref_get(&psec->refcnt);
 185                         return psec;
 186                 }
 187         }
 188
 189         psec = kzalloc(sizeof(*psec), GFP_KERNEL);
 190         if (!psec)
 191                 return ERR_PTR(-ENOMEM);
 192
 193         if (!try_module_get(pcdev->owner)) {
 194                 kfree(psec);
 195                 return ERR_PTR(-ENODEV);
 196         }
 197
 198         psec->pcdev = pcdev;
 199         list_add(&psec->list, &pcdev->pse_control_head);
 200         psec->id = index;
 201         kref_init(&psec->refcnt);
 202
 203         return psec;
 204 }
 205
 206 struct pse_control *
 207 of_pse_control_get(struct device_node *node)
 208 {
 209         struct pse_controller_dev *r, *pcdev;
 210         struct of_phandle_args args;
 211         struct pse_control *psec;
 212         int psec_id;
 213         int ret;
 214
 215         if (!node)
 216                 return ERR_PTR(-EINVAL);
 217
 218         ret = of_parse_phandle_with_args(node, "pses", "#pse-cells", 0, &args);
 219         if (ret)
 220                 return ERR_PTR(ret);
 221
 222         mutex_lock(&pse_list_mutex);
 223         pcdev = NULL;
 224         list_for_each_entry(r, &pse_controller_list, list) {
 225                 if (args.np == r->dev->of_node) {
 226                         pcdev = r;
 227                         break;
 228                 }
 229         }
 230
 231         if (!pcdev) {
 232                 psec = ERR_PTR(-EPROBE_DEFER);
 233                 goto out;
 234         }
 235
 236         if (WARN_ON(args.args_count != pcdev->of_pse_n_cells)) {
 237                 psec = ERR_PTR(-EINVAL);
 238                 goto out;
 239         }
 240
 241         psec_id = pcdev->of_xlate(pcdev, &args);
 242         if (psec_id < 0) {
 243                 psec = ERR_PTR(psec_id);
 244                 goto out;
 245         }
 246
 247         /* pse_list_mutex also protects the pcdev's pse_control list */
 248         psec = pse_control_get_internal(pcdev, psec_id);
 249
 250 out:
 251         mutex_unlock(&pse_list_mutex);
 252         of_node_put(args.np);
 253
 254         return psec;
 255 }
 256 EXPORT_SYMBOL_GPL(of_pse_control_get);
 257
 258 /**
 259  * pse_ethtool_get_status - get status of PSE control
 260  * @psec: PSE control pointer
 261  * @extack: extack for reporting useful error messages
 262  * @status: struct to store PSE status
 263  */
 264 int pse_ethtool_get_status(struct pse_control *psec,
 265                            struct netlink_ext_ack *extack,
 266                            struct pse_control_status *status)
 267 {
 268         const struct pse_controller_ops *ops;
 269         int err;
 270
 271         ops = psec->pcdev->ops;
 272
 273         if (!ops->ethtool_get_status) {
 274                 NL_SET_ERR_MSG(extack,
 275                                "PSE driver does not support status report");
 276                 return -EOPNOTSUPP;
 277         }
 278
 279         mutex_lock(&psec->pcdev->lock);
 280         err = ops->ethtool_get_status(psec->pcdev, psec->id, extack, status);
 281         mutex_unlock(&psec->pcdev->lock);
 282
 283         return err;
 284 }
 285 EXPORT_SYMBOL_GPL(pse_ethtool_get_status);
 286
 287 /**
 288  * pse_ethtool_set_config - set PSE control configuration
 289  * @psec: PSE control pointer
 290  * @extack: extack for reporting useful error messages
 291  * @config: Configuration of the test to run
 292  */
 293 int pse_ethtool_set_config(struct pse_control *psec,
 294                            struct netlink_ext_ack *extack,
 295                            const struct pse_control_config *config)
 296 {
 297         const struct pse_controller_ops *ops;
 298         int err;
 299
 300         ops = psec->pcdev->ops;
 301
 302         if (!ops->ethtool_set_config) {
 303                 NL_SET_ERR_MSG(extack,
 304                                "PSE driver does not configuration");
 305                 return -EOPNOTSUPP;
 306         }
 307
 308         mutex_lock(&psec->pcdev->lock);
 309         err = ops->ethtool_set_config(psec->pcdev, psec->id, extack, config);
 310         mutex_unlock(&psec->pcdev->lock);
 311
 312         return err;
 313 }
 314 EXPORT_SYMBOL_GPL(pse_ethtool_set_config);