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