1 // SPDX-License-Identifier: GPL-2.0-only 2 // 3 // ethtool interface 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 16 struct pse_req_info { 17 struct ethnl_req_info base; 18 }; 19 20 struct pse_reply_data { 21 struct ethnl_reply_data base; 22 struct ethtool_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 30 const struct nla_policy ethnl_pse_get_policy[ETHTOOL_A_PSE_HEADER + 1] = { 31 [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy_phy), 32 }; 33 34 static int pse_get_pse_attributes(struct phy_device *phydev, 35 struct netlink_ext_ack *extack, 36 struct pse_reply_data *data) 37 { 38 if (!phydev) { 39 NL_SET_ERR_MSG(extack, "No PHY found"); 40 return -EOPNOTSUPP; 41 } 42 43 if (!phydev->psec) { 44 NL_SET_ERR_MSG(extack, "No PSE is attached"); 45 return -EOPNOTSUPP; 46 } 47 48 memset(&data->status, 0, sizeof(data->status)); 49 50 return pse_ethtool_get_status(phydev->psec, extack, &data->status); 51 } 52 53 static int pse_prepare_data(const struct ethnl_req_info *req_base, 54 struct ethnl_reply_data *reply_base, 55 const struct genl_info *info) 56 { 57 struct pse_reply_data *data = PSE_REPDATA(reply_base); 58 struct net_device *dev = reply_base->dev; 59 struct nlattr **tb = info->attrs; 60 struct phy_device *phydev; 61 int ret; 62 63 ret = ethnl_ops_begin(dev); 64 if (ret < 0) 65 return ret; 66 67 phydev = ethnl_req_get_phydev(req_base, tb, ETHTOOL_A_PSE_HEADER, 68 info->extack); 69 if (IS_ERR(phydev)) 70 return -ENODEV; 71 72 ret = pse_get_pse_attributes(phydev, info->extack, data); 73 74 ethnl_ops_complete(dev); 75 76 return ret; 77 } 78 79 static int pse_reply_size(const struct ethnl_req_info *req_base, 80 const struct ethnl_reply_data *reply_base) 81 { 82 const struct pse_reply_data *data = PSE_REPDATA(reply_base); 83 const struct ethtool_pse_control_status *st = &data->status; 84 int len = 0; 85 86 if (st->pw_d_id) 87 len += nla_total_size(sizeof(u32)); /* _PSE_PW_D_ID */ 88 if (st->podl_admin_state > 0) 89 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_ADMIN_STATE */ 90 if (st->podl_pw_status > 0) 91 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_PW_D_STATUS */ 92 if (st->c33_admin_state > 0) 93 len += nla_total_size(sizeof(u32)); /* _C33_PSE_ADMIN_STATE */ 94 if (st->c33_pw_status > 0) 95 len += nla_total_size(sizeof(u32)); /* _C33_PSE_PW_D_STATUS */ 96 if (st->c33_pw_class > 0) 97 len += nla_total_size(sizeof(u32)); /* _C33_PSE_PW_CLASS */ 98 if (st->c33_actual_pw > 0) 99 len += nla_total_size(sizeof(u32)); /* _C33_PSE_ACTUAL_PW */ 100 if (st->c33_ext_state_info.c33_pse_ext_state > 0) { 101 len += nla_total_size(sizeof(u32)); /* _C33_PSE_EXT_STATE */ 102 if (st->c33_ext_state_info.__c33_pse_ext_substate > 0) 103 /* _C33_PSE_EXT_SUBSTATE */ 104 len += nla_total_size(sizeof(u32)); 105 } 106 if (st->c33_avail_pw_limit > 0) 107 /* _C33_AVAIL_PSE_PW_LIMIT */ 108 len += nla_total_size(sizeof(u32)); 109 if (st->c33_pw_limit_nb_ranges > 0) 110 /* _C33_PSE_PW_LIMIT_RANGES */ 111 len += st->c33_pw_limit_nb_ranges * 112 (nla_total_size(0) + 113 nla_total_size(sizeof(u32)) * 2); 114 if (st->prio_max) 115 /* _PSE_PRIO_MAX + _PSE_PRIO */ 116 len += nla_total_size(sizeof(u32)) * 2; 117 118 return len; 119 } 120 121 static int pse_put_pw_limit_ranges(struct sk_buff *skb, 122 const struct ethtool_pse_control_status *st) 123 { 124 const struct ethtool_c33_pse_pw_limit_range *pw_limit_ranges; 125 int i; 126 127 pw_limit_ranges = st->c33_pw_limit_ranges; 128 for (i = 0; i < st->c33_pw_limit_nb_ranges; i++) { 129 struct nlattr *nest; 130 131 nest = nla_nest_start(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_RANGES); 132 if (!nest) 133 return -EMSGSIZE; 134 135 if (nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_MIN, 136 pw_limit_ranges->min) || 137 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_MAX, 138 pw_limit_ranges->max)) { 139 nla_nest_cancel(skb, nest); 140 return -EMSGSIZE; 141 } 142 nla_nest_end(skb, nest); 143 pw_limit_ranges++; 144 } 145 146 return 0; 147 } 148 149 static int pse_fill_reply(struct sk_buff *skb, 150 const struct ethnl_req_info *req_base, 151 const struct ethnl_reply_data *reply_base) 152 { 153 const struct pse_reply_data *data = PSE_REPDATA(reply_base); 154 const struct ethtool_pse_control_status *st = &data->status; 155 156 if (st->pw_d_id && 157 nla_put_u32(skb, ETHTOOL_A_PSE_PW_D_ID, 158 st->pw_d_id)) 159 return -EMSGSIZE; 160 161 if (st->podl_admin_state > 0 && 162 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_ADMIN_STATE, 163 st->podl_admin_state)) 164 return -EMSGSIZE; 165 166 if (st->podl_pw_status > 0 && 167 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_PW_D_STATUS, 168 st->podl_pw_status)) 169 return -EMSGSIZE; 170 171 if (st->c33_admin_state > 0 && 172 nla_put_u32(skb, ETHTOOL_A_C33_PSE_ADMIN_STATE, 173 st->c33_admin_state)) 174 return -EMSGSIZE; 175 176 if (st->c33_pw_status > 0 && 177 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_D_STATUS, 178 st->c33_pw_status)) 179 return -EMSGSIZE; 180 181 if (st->c33_pw_class > 0 && 182 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_CLASS, 183 st->c33_pw_class)) 184 return -EMSGSIZE; 185 186 if (st->c33_actual_pw > 0 && 187 nla_put_u32(skb, ETHTOOL_A_C33_PSE_ACTUAL_PW, 188 st->c33_actual_pw)) 189 return -EMSGSIZE; 190 191 if (st->c33_ext_state_info.c33_pse_ext_state > 0) { 192 if (nla_put_u32(skb, ETHTOOL_A_C33_PSE_EXT_STATE, 193 st->c33_ext_state_info.c33_pse_ext_state)) 194 return -EMSGSIZE; 195 196 if (st->c33_ext_state_info.__c33_pse_ext_substate > 0 && 197 nla_put_u32(skb, ETHTOOL_A_C33_PSE_EXT_SUBSTATE, 198 st->c33_ext_state_info.__c33_pse_ext_substate)) 199 return -EMSGSIZE; 200 } 201 202 if (st->c33_avail_pw_limit > 0 && 203 nla_put_u32(skb, ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT, 204 st->c33_avail_pw_limit)) 205 return -EMSGSIZE; 206 207 if (st->c33_pw_limit_nb_ranges > 0 && 208 pse_put_pw_limit_ranges(skb, st)) 209 return -EMSGSIZE; 210 211 if (st->prio_max && 212 (nla_put_u32(skb, ETHTOOL_A_PSE_PRIO_MAX, st->prio_max) || 213 nla_put_u32(skb, ETHTOOL_A_PSE_PRIO, st->prio))) 214 return -EMSGSIZE; 215 216 return 0; 217 } 218 219 static void pse_cleanup_data(struct ethnl_reply_data *reply_base) 220 { 221 const struct pse_reply_data *data = PSE_REPDATA(reply_base); 222 223 kfree(data->status.c33_pw_limit_ranges); 224 } 225 226 /* PSE_SET */ 227 228 const struct nla_policy ethnl_pse_set_policy[ETHTOOL_A_PSE_MAX + 1] = { 229 [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy_phy), 230 [ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] = 231 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED, 232 ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED), 233 [ETHTOOL_A_C33_PSE_ADMIN_CONTROL] = 234 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED, 235 ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED), 236 [ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT] = { .type = NLA_U32 }, 237 [ETHTOOL_A_PSE_PRIO] = { .type = NLA_U32 }, 238 }; 239 240 static int 241 ethnl_set_pse_validate(struct phy_device *phydev, struct genl_info *info) 242 { 243 struct nlattr **tb = info->attrs; 244 245 if (IS_ERR_OR_NULL(phydev)) { 246 NL_SET_ERR_MSG(info->extack, "No PHY is attached"); 247 return -EOPNOTSUPP; 248 } 249 250 if (!phydev->psec) { 251 NL_SET_ERR_MSG(info->extack, "No PSE is attached"); 252 return -EOPNOTSUPP; 253 } 254 255 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] && 256 !pse_has_podl(phydev->psec)) { 257 NL_SET_ERR_MSG_ATTR(info->extack, 258 tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL], 259 "setting PoDL PSE admin control not supported"); 260 return -EOPNOTSUPP; 261 } 262 if (tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL] && 263 !pse_has_c33(phydev->psec)) { 264 NL_SET_ERR_MSG_ATTR(info->extack, 265 tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL], 266 "setting C33 PSE admin control not supported"); 267 return -EOPNOTSUPP; 268 } 269 270 return 0; 271 } 272 273 static int 274 ethnl_set_pse(struct ethnl_req_info *req_info, struct genl_info *info) 275 { 276 struct nlattr **tb = info->attrs; 277 struct phy_device *phydev; 278 int ret; 279 280 phydev = ethnl_req_get_phydev(req_info, tb, ETHTOOL_A_PSE_HEADER, 281 info->extack); 282 ret = ethnl_set_pse_validate(phydev, info); 283 if (ret) 284 return ret; 285 286 if (tb[ETHTOOL_A_PSE_PRIO]) { 287 unsigned int prio; 288 289 prio = nla_get_u32(tb[ETHTOOL_A_PSE_PRIO]); 290 ret = pse_ethtool_set_prio(phydev->psec, info->extack, prio); 291 if (ret) 292 return ret; 293 } 294 295 if (tb[ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT]) { 296 unsigned int pw_limit; 297 298 pw_limit = nla_get_u32(tb[ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT]); 299 ret = pse_ethtool_set_pw_limit(phydev->psec, info->extack, 300 pw_limit); 301 if (ret) 302 return ret; 303 } 304 305 /* These values are already validated by the ethnl_pse_set_policy */ 306 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] || 307 tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]) { 308 struct pse_control_config config = {}; 309 310 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]) 311 config.podl_admin_control = nla_get_u32(tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]); 312 if (tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]) 313 config.c33_admin_control = nla_get_u32(tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]); 314 315 /* pse_ethtool_set_config() will do nothing if the config 316 * is zero 317 */ 318 ret = pse_ethtool_set_config(phydev->psec, info->extack, 319 &config); 320 if (ret) 321 return ret; 322 } 323 324 /* Return errno or zero - PSE has no notification */ 325 return ret; 326 } 327 328 const struct ethnl_request_ops ethnl_pse_request_ops = { 329 .request_cmd = ETHTOOL_MSG_PSE_GET, 330 .reply_cmd = ETHTOOL_MSG_PSE_GET_REPLY, 331 .hdr_attr = ETHTOOL_A_PSE_HEADER, 332 .req_info_size = sizeof(struct pse_req_info), 333 .reply_data_size = sizeof(struct pse_reply_data), 334 335 .prepare_data = pse_prepare_data, 336 .reply_size = pse_reply_size, 337 .fill_reply = pse_fill_reply, 338 .cleanup_data = pse_cleanup_data, 339 340 .set = ethnl_set_pse, 341 /* PSE has no notification */ 342 }; 343 344 void ethnl_pse_send_ntf(struct net_device *netdev, unsigned long notifs) 345 { 346 void *reply_payload; 347 struct sk_buff *skb; 348 int reply_len; 349 int ret; 350 351 ASSERT_RTNL(); 352 353 if (!netdev || !notifs) 354 return; 355 356 reply_len = ethnl_reply_header_size() + 357 nla_total_size(sizeof(u32)); /* _PSE_NTF_EVENTS */ 358 359 skb = genlmsg_new(reply_len, GFP_KERNEL); 360 if (!skb) 361 return; 362 363 reply_payload = ethnl_bcastmsg_put(skb, ETHTOOL_MSG_PSE_NTF); 364 if (!reply_payload) 365 goto err_skb; 366 367 ret = ethnl_fill_reply_header(skb, netdev, ETHTOOL_A_PSE_NTF_HEADER); 368 if (ret < 0) 369 goto err_skb; 370 371 if (nla_put_uint(skb, ETHTOOL_A_PSE_NTF_EVENTS, notifs)) 372 goto err_skb; 373 374 genlmsg_end(skb, reply_payload); 375 ethnl_multicast(skb, netdev); 376 return; 377 378 err_skb: 379 nlmsg_free(skb); 380 } 381 EXPORT_SYMBOL_GPL(ethnl_pse_send_ntf); 382