ethtool/pse-pd.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 //
   3 // ethtool interface for 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                                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 ? info->extack : NULL, 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 const struct ethnl_request_ops ethnl_pse_request_ops = {
 110         .request_cmd            = ETHTOOL_MSG_PSE_GET,
 111         .reply_cmd              = ETHTOOL_MSG_PSE_GET_REPLY,
 112         .hdr_attr               = ETHTOOL_A_PSE_HEADER,
 113         .req_info_size          = sizeof(struct pse_req_info),
 114         .reply_data_size        = sizeof(struct pse_reply_data),
 115
 116         .prepare_data           = pse_prepare_data,
 117         .reply_size             = pse_reply_size,
 118         .fill_reply             = pse_fill_reply,
 119 };
 120
 121 /* PSE_SET */
 122
 123 const struct nla_policy ethnl_pse_set_policy[ETHTOOL_A_PSE_MAX + 1] = {
 124         [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
 125         [ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] =
 126                 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED,
 127                                  ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED),
 128 };
 129
 130 static int pse_set_pse_config(struct net_device *dev,
 131                               struct netlink_ext_ack *extack,
 132                               struct nlattr **tb)
 133 {
 134         struct phy_device *phydev = dev->phydev;
 135         struct pse_control_config config = {};
 136
 137         /* Optional attribute. Do not return error if not set. */
 138         if (!tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL])
 139                 return 0;
 140
 141         /* this values are already validated by the ethnl_pse_set_policy */
 142         config.admin_cotrol = nla_get_u32(tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]);
 143
 144         if (!phydev) {
 145                 NL_SET_ERR_MSG(extack, "No PHY is attached");
 146                 return -EOPNOTSUPP;
 147         }
 148
 149         if (!phydev->psec) {
 150                 NL_SET_ERR_MSG(extack, "No PSE is attached");
 151                 return -EOPNOTSUPP;
 152         }
 153
 154         return pse_ethtool_set_config(phydev->psec, extack, &config);
 155 }
 156
 157 int ethnl_set_pse(struct sk_buff *skb, struct genl_info *info)
 158 {
 159         struct ethnl_req_info req_info = {};
 160         struct nlattr **tb = info->attrs;
 161         struct net_device *dev;
 162         int ret;
 163
 164         ret = ethnl_parse_header_dev_get(&req_info, tb[ETHTOOL_A_PSE_HEADER],
 165                                          genl_info_net(info), info->extack,
 166                                          true);
 167         if (ret < 0)
 168                 return ret;
 169
 170         dev = req_info.dev;
 171
 172         rtnl_lock();
 173         ret = ethnl_ops_begin(dev);
 174         if (ret < 0)
 175                 goto out_rtnl;
 176
 177         ret = pse_set_pse_config(dev, info->extack, tb);
 178         ethnl_ops_complete(dev);
 179 out_rtnl:
 180         rtnl_unlock();
 181
 182         ethnl_parse_header_dev_put(&req_info);
 183
 184         return ret;
 185 }