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