1 // SPDX-License-Identifier: GPL-2.0-only 2 3 #include <linux/ethtool.h> 4 #include <linux/phy.h> 5 #include <linux/slab.h> 6 7 #include "netlink.h" 8 #include "common.h" 9 10 /* Channels A-D only; WORST and LINK are exclusive alternatives */ 11 #define PHY_MSE_CHANNEL_COUNT 4 12 13 struct mse_req_info { 14 struct ethnl_req_info base; 15 }; 16 17 struct mse_snapshot_entry { 18 struct phy_mse_snapshot snapshot; 19 int channel; 20 }; 21 22 struct mse_reply_data { 23 struct ethnl_reply_data base; 24 struct phy_mse_capability capability; 25 struct mse_snapshot_entry *snapshots; 26 unsigned int num_snapshots; 27 }; 28 29 static struct mse_reply_data * 30 mse_repdata(const struct ethnl_reply_data *reply_base) 31 { 32 return container_of(reply_base, struct mse_reply_data, base); 33 } 34 35 const struct nla_policy ethnl_mse_get_policy[] = { 36 [ETHTOOL_A_MSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy_phy), 37 }; 38 39 static int get_snapshot_if_supported(struct phy_device *phydev, 40 struct mse_reply_data *data, 41 unsigned int *idx, u32 cap_bit, 42 enum phy_mse_channel channel) 43 { 44 int ret; 45 46 if (data->capability.supported_caps & cap_bit) { 47 ret = phydev->drv->get_mse_snapshot(phydev, channel, 48 &data->snapshots[*idx].snapshot); 49 if (ret) 50 return ret; 51 data->snapshots[*idx].channel = channel; 52 (*idx)++; 53 } 54 55 return 0; 56 } 57 58 static int mse_get_channels(struct phy_device *phydev, 59 struct mse_reply_data *data) 60 { 61 unsigned int i = 0; 62 int ret; 63 64 if (!data->capability.supported_caps) 65 return 0; 66 67 data->snapshots = kzalloc_objs(*data->snapshots, PHY_MSE_CHANNEL_COUNT); 68 if (!data->snapshots) 69 return -ENOMEM; 70 71 /* Priority 1: Individual channels */ 72 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_A, 73 PHY_MSE_CHANNEL_A); 74 if (ret) 75 return ret; 76 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_B, 77 PHY_MSE_CHANNEL_B); 78 if (ret) 79 return ret; 80 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_C, 81 PHY_MSE_CHANNEL_C); 82 if (ret) 83 return ret; 84 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_CHANNEL_D, 85 PHY_MSE_CHANNEL_D); 86 if (ret) 87 return ret; 88 89 /* If any individual channels were found, we are done. */ 90 if (i > 0) { 91 data->num_snapshots = i; 92 return 0; 93 } 94 95 /* Priority 2: Worst channel, if no individual channels supported. */ 96 ret = get_snapshot_if_supported(phydev, data, &i, 97 PHY_MSE_CAP_WORST_CHANNEL, 98 PHY_MSE_CHANNEL_WORST); 99 if (ret) 100 return ret; 101 102 /* If worst channel was found, we are done. */ 103 if (i > 0) { 104 data->num_snapshots = i; 105 return 0; 106 } 107 108 /* Priority 3: Link-wide, if nothing else is supported. */ 109 ret = get_snapshot_if_supported(phydev, data, &i, PHY_MSE_CAP_LINK, 110 PHY_MSE_CHANNEL_LINK); 111 if (ret) 112 return ret; 113 114 data->num_snapshots = i; 115 return 0; 116 } 117 118 static int mse_prepare_data(const struct ethnl_req_info *req_base, 119 struct ethnl_reply_data *reply_base, 120 const struct genl_info *info) 121 { 122 struct mse_reply_data *data = mse_repdata(reply_base); 123 struct net_device *dev = reply_base->dev; 124 struct phy_device *phydev; 125 int ret; 126 127 phydev = ethnl_req_get_phydev(req_base, info->attrs, 128 ETHTOOL_A_MSE_HEADER, info->extack); 129 if (IS_ERR(phydev)) 130 return PTR_ERR(phydev); 131 if (!phydev) 132 return -EOPNOTSUPP; 133 134 ret = ethnl_ops_begin(dev); 135 if (ret) 136 return ret; 137 138 mutex_lock(&phydev->lock); 139 140 if (!phydev->drv || !phydev->drv->get_mse_capability || 141 !phydev->drv->get_mse_snapshot) { 142 ret = -EOPNOTSUPP; 143 goto out_unlock; 144 } 145 if (!phydev->link) { 146 ret = -ENETDOWN; 147 goto out_unlock; 148 } 149 150 ret = phydev->drv->get_mse_capability(phydev, &data->capability); 151 if (ret) 152 goto out_unlock; 153 154 ret = mse_get_channels(phydev, data); 155 156 out_unlock: 157 mutex_unlock(&phydev->lock); 158 ethnl_ops_complete(dev); 159 if (ret) 160 kfree(data->snapshots); 161 return ret; 162 } 163 164 static void mse_cleanup_data(struct ethnl_reply_data *reply_base) 165 { 166 struct mse_reply_data *data = mse_repdata(reply_base); 167 168 kfree(data->snapshots); 169 } 170 171 static int mse_reply_size(const struct ethnl_req_info *req_base, 172 const struct ethnl_reply_data *reply_base) 173 { 174 const struct mse_reply_data *data = mse_repdata(reply_base); 175 size_t len = 0; 176 unsigned int i; 177 178 /* ETHTOOL_A_MSE_CAPABILITIES */ 179 len += nla_total_size(0); 180 if (data->capability.supported_caps & PHY_MSE_CAP_AVG) 181 /* ETHTOOL_A_MSE_CAPABILITIES_MAX_AVERAGE_MSE */ 182 len += nla_total_size(sizeof(u64)); 183 if (data->capability.supported_caps & (PHY_MSE_CAP_PEAK | 184 PHY_MSE_CAP_WORST_PEAK)) 185 /* ETHTOOL_A_MSE_CAPABILITIES_MAX_PEAK_MSE */ 186 len += nla_total_size(sizeof(u64)); 187 /* ETHTOOL_A_MSE_CAPABILITIES_REFRESH_RATE_PS */ 188 len += nla_total_size(sizeof(u64)); 189 /* ETHTOOL_A_MSE_CAPABILITIES_NUM_SYMBOLS */ 190 len += nla_total_size(sizeof(u64)); 191 192 for (i = 0; i < data->num_snapshots; i++) { 193 size_t snapshot_len = 0; 194 195 /* Per-channel nest (e.g., ETHTOOL_A_MSE_CHANNEL_A / _B / _C / 196 * _D / _WORST_CHANNEL / _LINK) 197 */ 198 snapshot_len += nla_total_size(0); 199 200 if (data->capability.supported_caps & PHY_MSE_CAP_AVG) 201 snapshot_len += nla_total_size(sizeof(u64)); 202 if (data->capability.supported_caps & PHY_MSE_CAP_PEAK) 203 snapshot_len += nla_total_size(sizeof(u64)); 204 if (data->capability.supported_caps & PHY_MSE_CAP_WORST_PEAK) 205 snapshot_len += nla_total_size(sizeof(u64)); 206 207 len += snapshot_len; 208 } 209 210 return len; 211 } 212 213 static int mse_channel_to_attr(int ch) 214 { 215 switch (ch) { 216 case PHY_MSE_CHANNEL_A: 217 return ETHTOOL_A_MSE_CHANNEL_A; 218 case PHY_MSE_CHANNEL_B: 219 return ETHTOOL_A_MSE_CHANNEL_B; 220 case PHY_MSE_CHANNEL_C: 221 return ETHTOOL_A_MSE_CHANNEL_C; 222 case PHY_MSE_CHANNEL_D: 223 return ETHTOOL_A_MSE_CHANNEL_D; 224 case PHY_MSE_CHANNEL_WORST: 225 return ETHTOOL_A_MSE_WORST_CHANNEL; 226 case PHY_MSE_CHANNEL_LINK: 227 return ETHTOOL_A_MSE_LINK; 228 default: 229 return -EINVAL; 230 } 231 } 232 233 static int mse_fill_reply(struct sk_buff *skb, 234 const struct ethnl_req_info *req_base, 235 const struct ethnl_reply_data *reply_base) 236 { 237 const struct mse_reply_data *data = mse_repdata(reply_base); 238 struct nlattr *nest; 239 unsigned int i; 240 int ret; 241 242 nest = nla_nest_start(skb, ETHTOOL_A_MSE_CAPABILITIES); 243 if (!nest) 244 return -EMSGSIZE; 245 246 if (data->capability.supported_caps & PHY_MSE_CAP_AVG) { 247 ret = nla_put_uint(skb, 248 ETHTOOL_A_MSE_CAPABILITIES_MAX_AVERAGE_MSE, 249 data->capability.max_average_mse); 250 if (ret < 0) 251 goto nla_put_nest_failure; 252 } 253 254 if (data->capability.supported_caps & (PHY_MSE_CAP_PEAK | 255 PHY_MSE_CAP_WORST_PEAK)) { 256 ret = nla_put_uint(skb, ETHTOOL_A_MSE_CAPABILITIES_MAX_PEAK_MSE, 257 data->capability.max_peak_mse); 258 if (ret < 0) 259 goto nla_put_nest_failure; 260 } 261 262 ret = nla_put_uint(skb, ETHTOOL_A_MSE_CAPABILITIES_REFRESH_RATE_PS, 263 data->capability.refresh_rate_ps); 264 if (ret < 0) 265 goto nla_put_nest_failure; 266 267 ret = nla_put_uint(skb, ETHTOOL_A_MSE_CAPABILITIES_NUM_SYMBOLS, 268 data->capability.num_symbols); 269 if (ret < 0) 270 goto nla_put_nest_failure; 271 272 nla_nest_end(skb, nest); 273 274 for (i = 0; i < data->num_snapshots; i++) { 275 const struct mse_snapshot_entry *s = &data->snapshots[i]; 276 int chan_attr; 277 278 chan_attr = mse_channel_to_attr(s->channel); 279 if (chan_attr < 0) 280 return chan_attr; 281 282 nest = nla_nest_start(skb, chan_attr); 283 if (!nest) 284 return -EMSGSIZE; 285 286 if (data->capability.supported_caps & PHY_MSE_CAP_AVG) { 287 ret = nla_put_uint(skb, 288 ETHTOOL_A_MSE_SNAPSHOT_AVERAGE_MSE, 289 s->snapshot.average_mse); 290 if (ret) 291 goto nla_put_nest_failure; 292 } 293 if (data->capability.supported_caps & PHY_MSE_CAP_PEAK) { 294 ret = nla_put_uint(skb, ETHTOOL_A_MSE_SNAPSHOT_PEAK_MSE, 295 s->snapshot.peak_mse); 296 if (ret) 297 goto nla_put_nest_failure; 298 } 299 if (data->capability.supported_caps & PHY_MSE_CAP_WORST_PEAK) { 300 ret = nla_put_uint(skb, 301 ETHTOOL_A_MSE_SNAPSHOT_WORST_PEAK_MSE, 302 s->snapshot.worst_peak_mse); 303 if (ret) 304 goto nla_put_nest_failure; 305 } 306 307 nla_nest_end(skb, nest); 308 } 309 310 return 0; 311 312 nla_put_nest_failure: 313 nla_nest_cancel(skb, nest); 314 return ret; 315 } 316 317 const struct ethnl_request_ops ethnl_mse_request_ops = { 318 .request_cmd = ETHTOOL_MSG_MSE_GET, 319 .reply_cmd = ETHTOOL_MSG_MSE_GET_REPLY, 320 .hdr_attr = ETHTOOL_A_MSE_HEADER, 321 .req_info_size = sizeof(struct mse_req_info), 322 .reply_data_size = sizeof(struct mse_reply_data), 323 324 .prepare_data = mse_prepare_data, 325 .cleanup_data = mse_cleanup_data, 326 .reply_size = mse_reply_size, 327 .fill_reply = mse_fill_reply, 328 }; 329