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