net/ethtool/pse-pd.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 //
   3 // ethtool interface for Ethernet PSE (Power Sourcing Equipment)
   4 // and PD (Powered Device)
   5 //
   6 // Copyright (c) 2022 Pengutronix, Oleksij Rempel <kernel@pengutronix.de>
   7 //
   8
   9 #include "common.h"
  10 #include "linux/pse-pd/pse.h"
  11 #include "netlink.h"
  12 #include <linux/ethtool_netlink.h>
  13 #include <linux/ethtool.h>
  14 #include <linux/phy.h>
  15
  16 struct pse_req_info {
  17         struct ethnl_req_info base;
  18 };
  19
  20 struct pse_reply_data {
  21         struct ethnl_reply_data base;
  22         struct pse_control_status status;
  23 };
  24
  25 #define PSE_REPDATA(__reply_base) \
  26         container_of(__reply_base, struct pse_reply_data, base)
  27
  28 /* PSE_GET */
  29
  30 const struct nla_policy ethnl_pse_get_policy[ETHTOOL_A_PSE_HEADER + 1] = {
  31         [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
  32 };
  33
  34 static int pse_get_pse_attributes(struct net_device *dev,
  35                                   struct netlink_ext_ack *extack,
  36                                   struct pse_reply_data *data)
  37 {
  38         struct phy_device *phydev = dev->phydev;
  39
  40         if (!phydev) {
  41                 NL_SET_ERR_MSG(extack, "No PHY is attached");
  42                 return -EOPNOTSUPP;
  43         }
  44
  45         if (!phydev->psec) {
  46                 NL_SET_ERR_MSG(extack, "No PSE is attached");
  47                 return -EOPNOTSUPP;
  48         }
  49
  50         memset(&data->status, 0, sizeof(data->status));
  51
  52         return pse_ethtool_get_status(phydev->psec, extack, &data->status);
  53 }
  54
  55 static int pse_prepare_data(const struct ethnl_req_info *req_base,
  56                             struct ethnl_reply_data *reply_base,
  57                             const struct genl_info *info)
  58 {
  59         struct pse_reply_data *data = PSE_REPDATA(reply_base);
  60         struct net_device *dev = reply_base->dev;
  61         int ret;
  62
  63         ret = ethnl_ops_begin(dev);
  64         if (ret < 0)
  65                 return ret;
  66
  67         ret = pse_get_pse_attributes(dev, info->extack, data);
  68
  69         ethnl_ops_complete(dev);
  70
  71         return ret;
  72 }
  73
  74 static int pse_reply_size(const struct ethnl_req_info *req_base,
  75                           const struct ethnl_reply_data *reply_base)
  76 {
  77         const struct pse_reply_data *data = PSE_REPDATA(reply_base);
  78         const struct pse_control_status *st = &data->status;
  79         int len = 0;
  80
  81         if (st->podl_admin_state > 0)
  82                 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_ADMIN_STATE */
  83         if (st->podl_pw_status > 0)
  84                 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_PW_D_STATUS */
  85
  86         return len;
  87 }
  88
  89 static int pse_fill_reply(struct sk_buff *skb,
  90                           const struct ethnl_req_info *req_base,
  91                           const struct ethnl_reply_data *reply_base)
  92 {
  93         const struct pse_reply_data *data = PSE_REPDATA(reply_base);
  94         const struct pse_control_status *st = &data->status;
  95
  96         if (st->podl_admin_state > 0 &&
  97             nla_put_u32(skb, ETHTOOL_A_PODL_PSE_ADMIN_STATE,
  98                         st->podl_admin_state))
  99                 return -EMSGSIZE;
 100
 101         if (st->podl_pw_status > 0 &&
 102             nla_put_u32(skb, ETHTOOL_A_PODL_PSE_PW_D_STATUS,
 103                         st->podl_pw_status))
 104                 return -EMSGSIZE;
 105
 106         return 0;
 107 }
 108
 109 /* PSE_SET */
 110
 111 const struct nla_policy ethnl_pse_set_policy[ETHTOOL_A_PSE_MAX + 1] = {
 112         [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
 113         [ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] =
 114                 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED,
 115                                  ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED),
 116 };
 117
 118 static int
 119 ethnl_set_pse_validate(struct ethnl_req_info *req_info, struct genl_info *info)
 120 {
 121         return !!info->attrs[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL];
 122 }
 123
 124 static int
 125 ethnl_set_pse(struct ethnl_req_info *req_info, struct genl_info *info)
 126 {
 127         struct net_device *dev = req_info->dev;
 128         struct pse_control_config config = {};
 129         struct nlattr **tb = info->attrs;
 130         struct phy_device *phydev;
 131
 132         /* this values are already validated by the ethnl_pse_set_policy */
 133         config.admin_cotrol = nla_get_u32(tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]);
 134
 135         phydev = dev->phydev;
 136         if (!phydev) {
 137                 NL_SET_ERR_MSG(info->extack, "No PHY is attached");
 138                 return -EOPNOTSUPP;
 139         }
 140
 141         if (!phydev->psec) {
 142                 NL_SET_ERR_MSG(info->extack, "No PSE is attached");
 143                 return -EOPNOTSUPP;
 144         }
 145
 146         /* Return errno directly - PSE has no notification */
 147         return pse_ethtool_set_config(phydev->psec, info->extack, &config);
 148 }
 149
 150 const struct ethnl_request_ops ethnl_pse_request_ops = {
 151         .request_cmd            = ETHTOOL_MSG_PSE_GET,
 152         .reply_cmd              = ETHTOOL_MSG_PSE_GET_REPLY,
 153         .hdr_attr               = ETHTOOL_A_PSE_HEADER,
 154         .req_info_size          = sizeof(struct pse_req_info),
 155         .reply_data_size        = sizeof(struct pse_reply_data),
 156
 157         .prepare_data           = pse_prepare_data,
 158         .reply_size             = pse_reply_size,
 159         .fill_reply             = pse_fill_reply,
 160
 161         .set_validate           = ethnl_set_pse_validate,
 162         .set                    = ethnl_set_pse,
 163         /* PSE has no notification */
 164 };