1 // SPDX-License-Identifier: GPL-2.0-only
2
3 #include "netlink.h"
4 #include "common.h"
5 #include <linux/phy.h>
6
7 struct linkstate_req_info {
8 struct ethnl_req_info base;
9 };
10
11 struct linkstate_reply_data {
12 struct ethnl_reply_data base;
13 int link;
14 int sqi;
15 int sqi_max;
16 bool link_ext_state_provided;
17 struct ethtool_link_ext_state_info ethtool_link_ext_state_info;
18 };
19
20 #define LINKSTATE_REPDATA(__reply_base) \
21 container_of(__reply_base, struct linkstate_reply_data, base)
22
23 const struct nla_policy ethnl_linkstate_get_policy[] = {
24 [ETHTOOL_A_LINKSTATE_HEADER] =
25 NLA_POLICY_NESTED(ethnl_header_policy),
26 };
27
linkstate_get_sqi(struct net_device * dev)28 static int linkstate_get_sqi(struct net_device *dev)
29 {
30 struct phy_device *phydev = dev->phydev;
31 int ret;
32
33 if (!phydev)
34 return -EOPNOTSUPP;
35
36 mutex_lock(&phydev->lock);
37 if (!phydev->drv || !phydev->drv->get_sqi)
38 ret = -EOPNOTSUPP;
39 else
40 ret = phydev->drv->get_sqi(phydev);
41 mutex_unlock(&phydev->lock);
42
43 return ret;
44 }
45
linkstate_get_sqi_max(struct net_device * dev)46 static int linkstate_get_sqi_max(struct net_device *dev)
47 {
48 struct phy_device *phydev = dev->phydev;
49 int ret;
50
51 if (!phydev)
52 return -EOPNOTSUPP;
53
54 mutex_lock(&phydev->lock);
55 if (!phydev->drv || !phydev->drv->get_sqi_max)
56 ret = -EOPNOTSUPP;
57 else
58 ret = phydev->drv->get_sqi_max(phydev);
59 mutex_unlock(&phydev->lock);
60
61 return ret;
62 };
63
linkstate_get_link_ext_state(struct net_device * dev,struct linkstate_reply_data * data)64 static int linkstate_get_link_ext_state(struct net_device *dev,
65 struct linkstate_reply_data *data)
66 {
67 int err;
68
69 if (!dev->ethtool_ops->get_link_ext_state)
70 return -EOPNOTSUPP;
71
72 err = dev->ethtool_ops->get_link_ext_state(dev, &data->ethtool_link_ext_state_info);
73 if (err)
74 return err;
75
76 data->link_ext_state_provided = true;
77
78 return 0;
79 }
80
linkstate_prepare_data(const struct ethnl_req_info * req_base,struct ethnl_reply_data * reply_base,struct genl_info * info)81 static int linkstate_prepare_data(const struct ethnl_req_info *req_base,
82 struct ethnl_reply_data *reply_base,
83 struct genl_info *info)
84 {
85 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
86 struct net_device *dev = reply_base->dev;
87 int ret;
88
89 ret = ethnl_ops_begin(dev);
90 if (ret < 0)
91 return ret;
92 data->link = __ethtool_get_link(dev);
93
94 ret = linkstate_get_sqi(dev);
95 if (ret < 0 && ret != -EOPNOTSUPP)
96 goto out;
97 data->sqi = ret;
98
99 ret = linkstate_get_sqi_max(dev);
100 if (ret < 0 && ret != -EOPNOTSUPP)
101 goto out;
102 data->sqi_max = ret;
103
104 if (dev->flags & IFF_UP) {
105 ret = linkstate_get_link_ext_state(dev, data);
106 if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA)
107 goto out;
108 }
109
110 ret = 0;
111 out:
112 ethnl_ops_complete(dev);
113 return ret;
114 }
115
linkstate_reply_size(const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)116 static int linkstate_reply_size(const struct ethnl_req_info *req_base,
117 const struct ethnl_reply_data *reply_base)
118 {
119 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
120 int len;
121
122 len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */
123 + 0;
124
125 if (data->sqi != -EOPNOTSUPP)
126 len += nla_total_size(sizeof(u32));
127
128 if (data->sqi_max != -EOPNOTSUPP)
129 len += nla_total_size(sizeof(u32));
130
131 if (data->link_ext_state_provided)
132 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */
133
134 if (data->ethtool_link_ext_state_info.__link_ext_substate)
135 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */
136
137 return len;
138 }
139
linkstate_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_base,const struct ethnl_reply_data * reply_base)140 static int linkstate_fill_reply(struct sk_buff *skb,
141 const struct ethnl_req_info *req_base,
142 const struct ethnl_reply_data *reply_base)
143 {
144 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
145
146 if (data->link >= 0 &&
147 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link))
148 return -EMSGSIZE;
149
150 if (data->sqi != -EOPNOTSUPP &&
151 nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi))
152 return -EMSGSIZE;
153
154 if (data->sqi_max != -EOPNOTSUPP &&
155 nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX, data->sqi_max))
156 return -EMSGSIZE;
157
158 if (data->link_ext_state_provided) {
159 if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE,
160 data->ethtool_link_ext_state_info.link_ext_state))
161 return -EMSGSIZE;
162
163 if (data->ethtool_link_ext_state_info.__link_ext_substate &&
164 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE,
165 data->ethtool_link_ext_state_info.__link_ext_substate))
166 return -EMSGSIZE;
167 }
168
169 return 0;
170 }
171
172 const struct ethnl_request_ops ethnl_linkstate_request_ops = {
173 .request_cmd = ETHTOOL_MSG_LINKSTATE_GET,
174 .reply_cmd = ETHTOOL_MSG_LINKSTATE_GET_REPLY,
175 .hdr_attr = ETHTOOL_A_LINKSTATE_HEADER,
176 .req_info_size = sizeof(struct linkstate_req_info),
177 .reply_data_size = sizeof(struct linkstate_reply_data),
178
179 .prepare_data = linkstate_prepare_data,
180 .reply_size = linkstate_reply_size,
181 .fill_reply = linkstate_fill_reply,
182 };
183