1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Copyright 2023 Bootlin
4 *
5 */
6 #include <linux/phy.h>
7 #include <linux/phy_link_topology.h>
8 #include <linux/sfp.h>
9 #include <net/netdev_lock.h>
10
11 #include "common.h"
12 #include "netlink.h"
13
14 struct phy_req_info {
15 struct ethnl_req_info base;
16 };
17
18 struct phy_reply_data {
19 struct ethnl_reply_data base;
20 u32 phyindex;
21 char *drvname;
22 char *name;
23 unsigned int upstream_type;
24 char *upstream_sfp_name;
25 unsigned int upstream_index;
26 char *downstream_sfp_name;
27 };
28
29 #define PHY_REPDATA(__reply_base) \
30 container_of(__reply_base, struct phy_reply_data, base)
31
32 const struct nla_policy ethnl_phy_get_policy[ETHTOOL_A_PHY_HEADER + 1] = {
33 [ETHTOOL_A_PHY_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy),
34 };
35
phy_reply_size(const struct ethnl_req_info * req_info,const struct ethnl_reply_data * reply_data)36 static int phy_reply_size(const struct ethnl_req_info *req_info,
37 const struct ethnl_reply_data *reply_data)
38 {
39 struct phy_reply_data *rep_data = PHY_REPDATA(reply_data);
40 size_t size = 0;
41
42 /* ETHTOOL_A_PHY_INDEX */
43 size += nla_total_size(sizeof(u32));
44
45 /* ETHTOOL_A_DRVNAME */
46 if (rep_data->drvname)
47 size += nla_total_size(strlen(rep_data->drvname) + 1);
48
49 /* ETHTOOL_A_NAME */
50 size += nla_total_size(strlen(rep_data->name) + 1);
51
52 /* ETHTOOL_A_PHY_UPSTREAM_TYPE */
53 size += nla_total_size(sizeof(u32));
54
55 /* ETHTOOL_A_PHY_UPSTREAM_SFP_NAME */
56 if (rep_data->upstream_sfp_name)
57 size += nla_total_size(strlen(rep_data->upstream_sfp_name) + 1);
58
59 /* ETHTOOL_A_PHY_UPSTREAM_INDEX */
60 if (rep_data->upstream_index)
61 size += nla_total_size(sizeof(u32));
62
63 /* ETHTOOL_A_PHY_DOWNSTREAM_SFP_NAME */
64 if (rep_data->downstream_sfp_name)
65 size += nla_total_size(strlen(rep_data->downstream_sfp_name) + 1);
66
67 return size;
68 }
69
phy_prepare_data(const struct ethnl_req_info * req_info,struct ethnl_reply_data * reply_data,const struct genl_info * info)70 static int phy_prepare_data(const struct ethnl_req_info *req_info,
71 struct ethnl_reply_data *reply_data,
72 const struct genl_info *info)
73 {
74 struct phy_link_topology *topo = reply_data->dev->link_topo;
75 struct phy_reply_data *rep_data = PHY_REPDATA(reply_data);
76 struct nlattr **tb = info->attrs;
77 struct phy_device_node *pdn;
78 struct phy_device *phydev;
79 int ret;
80
81 /* RTNL is held by the caller */
82 phydev = ethnl_req_get_phydev(req_info, tb, ETHTOOL_A_PHY_HEADER,
83 info->extack);
84 if (IS_ERR_OR_NULL(phydev))
85 return -EOPNOTSUPP;
86
87 pdn = xa_load(&topo->phys, phydev->phyindex);
88 if (!pdn)
89 return -EOPNOTSUPP;
90
91 rep_data->phyindex = phydev->phyindex;
92
93 rep_data->name = kstrdup(dev_name(&phydev->mdio.dev), GFP_KERNEL);
94 if (!rep_data->name)
95 return -ENOMEM;
96
97 if (phydev->drv) {
98 rep_data->drvname = kstrdup(phydev->drv->name, GFP_KERNEL);
99 if (!rep_data->drvname) {
100 ret = -ENOMEM;
101 goto err_free_name;
102 }
103 }
104
105 rep_data->upstream_type = pdn->upstream_type;
106
107 if (pdn->upstream_type == PHY_UPSTREAM_PHY) {
108 struct phy_device *upstream = pdn->upstream.phydev;
109 rep_data->upstream_index = upstream->phyindex;
110 }
111
112 if (pdn->parent_sfp_bus) {
113 rep_data->upstream_sfp_name = kstrdup(sfp_get_name(pdn->parent_sfp_bus),
114 GFP_KERNEL);
115 if (!rep_data->upstream_sfp_name) {
116 ret = -ENOMEM;
117 goto err_free_drvname;
118 }
119 }
120
121 if (phydev->sfp_bus) {
122 rep_data->downstream_sfp_name = kstrdup(sfp_get_name(phydev->sfp_bus),
123 GFP_KERNEL);
124 if (!rep_data->downstream_sfp_name) {
125 ret = -ENOMEM;
126 goto err_free_upstream_sfp;
127 }
128 }
129
130 return 0;
131
132 err_free_upstream_sfp:
133 kfree(rep_data->upstream_sfp_name);
134 err_free_drvname:
135 kfree(rep_data->drvname);
136 err_free_name:
137 kfree(rep_data->name);
138 return ret;
139 }
140
phy_fill_reply(struct sk_buff * skb,const struct ethnl_req_info * req_info,const struct ethnl_reply_data * reply_data)141 static int phy_fill_reply(struct sk_buff *skb,
142 const struct ethnl_req_info *req_info,
143 const struct ethnl_reply_data *reply_data)
144 {
145 struct phy_reply_data *rep_data = PHY_REPDATA(reply_data);
146
147 if (nla_put_u32(skb, ETHTOOL_A_PHY_INDEX, rep_data->phyindex) ||
148 nla_put_string(skb, ETHTOOL_A_PHY_NAME, rep_data->name) ||
149 nla_put_u32(skb, ETHTOOL_A_PHY_UPSTREAM_TYPE, rep_data->upstream_type))
150 return -EMSGSIZE;
151
152 if (rep_data->drvname &&
153 nla_put_string(skb, ETHTOOL_A_PHY_DRVNAME, rep_data->drvname))
154 return -EMSGSIZE;
155
156 if (rep_data->upstream_index &&
157 nla_put_u32(skb, ETHTOOL_A_PHY_UPSTREAM_INDEX,
158 rep_data->upstream_index))
159 return -EMSGSIZE;
160
161 if (rep_data->upstream_sfp_name &&
162 nla_put_string(skb, ETHTOOL_A_PHY_UPSTREAM_SFP_NAME,
163 rep_data->upstream_sfp_name))
164 return -EMSGSIZE;
165
166 if (rep_data->downstream_sfp_name &&
167 nla_put_string(skb, ETHTOOL_A_PHY_DOWNSTREAM_SFP_NAME,
168 rep_data->downstream_sfp_name))
169 return -EMSGSIZE;
170
171 return 0;
172 }
173
phy_cleanup_data(struct ethnl_reply_data * reply_data)174 static void phy_cleanup_data(struct ethnl_reply_data *reply_data)
175 {
176 struct phy_reply_data *rep_data = PHY_REPDATA(reply_data);
177
178 kfree(rep_data->drvname);
179 kfree(rep_data->name);
180 kfree(rep_data->upstream_sfp_name);
181 kfree(rep_data->downstream_sfp_name);
182 }
183
184 const struct ethnl_request_ops ethnl_phy_request_ops = {
185 .request_cmd = ETHTOOL_MSG_PHY_GET,
186 .reply_cmd = ETHTOOL_MSG_PHY_GET_REPLY,
187 .hdr_attr = ETHTOOL_A_PHY_HEADER,
188 .req_info_size = sizeof(struct phy_req_info),
189 .reply_data_size = sizeof(struct phy_reply_data),
190
191 .prepare_data = phy_prepare_data,
192 .reply_size = phy_reply_size,
193 .fill_reply = phy_fill_reply,
194 .cleanup_data = phy_cleanup_data,
195 };
196