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
 16struct pse_req_info {
 17	struct ethnl_req_info base;
 18};
 19
 20struct 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
 30const 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
 34static 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
 55static 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
 74static 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
 89static 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
109const 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
123const 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
130static 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
157int 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);
179out_rtnl:
180	rtnl_unlock();
181
182	ethnl_parse_header_dev_put(&req_info);
183
184	return ret;
185}