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 115 return len; 116 } 117 118 static int pse_put_pw_limit_ranges(struct sk_buff *skb, 119 const struct ethtool_pse_control_status *st) 120 { 121 const struct ethtool_c33_pse_pw_limit_range *pw_limit_ranges; 122 int i; 123 124 pw_limit_ranges = st->c33_pw_limit_ranges; 125 for (i = 0; i < st->c33_pw_limit_nb_ranges; i++) { 126 struct nlattr *nest; 127 128 nest = nla_nest_start(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_RANGES); 129 if (!nest) 130 return -EMSGSIZE; 131 132 if (nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_MIN, 133 pw_limit_ranges->min) || 134 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_MAX, 135 pw_limit_ranges->max)) { 136 nla_nest_cancel(skb, nest); 137 return -EMSGSIZE; 138 } 139 nla_nest_end(skb, nest); 140 pw_limit_ranges++; 141 } 142 143 return 0; 144 } 145 146 static int pse_fill_reply(struct sk_buff *skb, 147 const struct ethnl_req_info *req_base, 148 const struct ethnl_reply_data *reply_base) 149 { 150 const struct pse_reply_data *data = PSE_REPDATA(reply_base); 151 const struct ethtool_pse_control_status *st = &data->status; 152 153 if (st->pw_d_id && 154 nla_put_u32(skb, ETHTOOL_A_PSE_PW_D_ID, 155 st->pw_d_id)) 156 return -EMSGSIZE; 157 158 if (st->podl_admin_state > 0 && 159 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_ADMIN_STATE, 160 st->podl_admin_state)) 161 return -EMSGSIZE; 162 163 if (st->podl_pw_status > 0 && 164 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_PW_D_STATUS, 165 st->podl_pw_status)) 166 return -EMSGSIZE; 167 168 if (st->c33_admin_state > 0 && 169 nla_put_u32(skb, ETHTOOL_A_C33_PSE_ADMIN_STATE, 170 st->c33_admin_state)) 171 return -EMSGSIZE; 172 173 if (st->c33_pw_status > 0 && 174 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_D_STATUS, 175 st->c33_pw_status)) 176 return -EMSGSIZE; 177 178 if (st->c33_pw_class > 0 && 179 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_CLASS, 180 st->c33_pw_class)) 181 return -EMSGSIZE; 182 183 if (st->c33_actual_pw > 0 && 184 nla_put_u32(skb, ETHTOOL_A_C33_PSE_ACTUAL_PW, 185 st->c33_actual_pw)) 186 return -EMSGSIZE; 187 188 if (st->c33_ext_state_info.c33_pse_ext_state > 0) { 189 if (nla_put_u32(skb, ETHTOOL_A_C33_PSE_EXT_STATE, 190 st->c33_ext_state_info.c33_pse_ext_state)) 191 return -EMSGSIZE; 192 193 if (st->c33_ext_state_info.__c33_pse_ext_substate > 0 && 194 nla_put_u32(skb, ETHTOOL_A_C33_PSE_EXT_SUBSTATE, 195 st->c33_ext_state_info.__c33_pse_ext_substate)) 196 return -EMSGSIZE; 197 } 198 199 if (st->c33_avail_pw_limit > 0 && 200 nla_put_u32(skb, ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT, 201 st->c33_avail_pw_limit)) 202 return -EMSGSIZE; 203 204 if (st->c33_pw_limit_nb_ranges > 0 && 205 pse_put_pw_limit_ranges(skb, st)) 206 return -EMSGSIZE; 207 208 return 0; 209 } 210 211 static void pse_cleanup_data(struct ethnl_reply_data *reply_base) 212 { 213 const struct pse_reply_data *data = PSE_REPDATA(reply_base); 214 215 kfree(data->status.c33_pw_limit_ranges); 216 } 217 218 /* PSE_SET */ 219 220 const struct nla_policy ethnl_pse_set_policy[ETHTOOL_A_PSE_MAX + 1] = { 221 [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy_phy), 222 [ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] = 223 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED, 224 ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED), 225 [ETHTOOL_A_C33_PSE_ADMIN_CONTROL] = 226 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED, 227 ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED), 228 [ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT] = { .type = NLA_U32 }, 229 }; 230 231 static int 232 ethnl_set_pse_validate(struct phy_device *phydev, struct genl_info *info) 233 { 234 struct nlattr **tb = info->attrs; 235 236 if (IS_ERR_OR_NULL(phydev)) { 237 NL_SET_ERR_MSG(info->extack, "No PHY is attached"); 238 return -EOPNOTSUPP; 239 } 240 241 if (!phydev->psec) { 242 NL_SET_ERR_MSG(info->extack, "No PSE is attached"); 243 return -EOPNOTSUPP; 244 } 245 246 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] && 247 !pse_has_podl(phydev->psec)) { 248 NL_SET_ERR_MSG_ATTR(info->extack, 249 tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL], 250 "setting PoDL PSE admin control not supported"); 251 return -EOPNOTSUPP; 252 } 253 if (tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL] && 254 !pse_has_c33(phydev->psec)) { 255 NL_SET_ERR_MSG_ATTR(info->extack, 256 tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL], 257 "setting C33 PSE admin control not supported"); 258 return -EOPNOTSUPP; 259 } 260 261 return 0; 262 } 263 264 static int 265 ethnl_set_pse(struct ethnl_req_info *req_info, struct genl_info *info) 266 { 267 struct nlattr **tb = info->attrs; 268 struct phy_device *phydev; 269 int ret; 270 271 phydev = ethnl_req_get_phydev(req_info, tb, ETHTOOL_A_PSE_HEADER, 272 info->extack); 273 ret = ethnl_set_pse_validate(phydev, info); 274 if (ret) 275 return ret; 276 277 if (tb[ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT]) { 278 unsigned int pw_limit; 279 280 pw_limit = nla_get_u32(tb[ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT]); 281 ret = pse_ethtool_set_pw_limit(phydev->psec, info->extack, 282 pw_limit); 283 if (ret) 284 return ret; 285 } 286 287 /* These values are already validated by the ethnl_pse_set_policy */ 288 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] || 289 tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]) { 290 struct pse_control_config config = {}; 291 292 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]) 293 config.podl_admin_control = nla_get_u32(tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]); 294 if (tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]) 295 config.c33_admin_control = nla_get_u32(tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]); 296 297 /* pse_ethtool_set_config() will do nothing if the config 298 * is zero 299 */ 300 ret = pse_ethtool_set_config(phydev->psec, info->extack, 301 &config); 302 if (ret) 303 return ret; 304 } 305 306 /* Return errno or zero - PSE has no notification */ 307 return ret; 308 } 309 310 const struct ethnl_request_ops ethnl_pse_request_ops = { 311 .request_cmd = ETHTOOL_MSG_PSE_GET, 312 .reply_cmd = ETHTOOL_MSG_PSE_GET_REPLY, 313 .hdr_attr = ETHTOOL_A_PSE_HEADER, 314 .req_info_size = sizeof(struct pse_req_info), 315 .reply_data_size = sizeof(struct pse_reply_data), 316 317 .prepare_data = pse_prepare_data, 318 .reply_size = pse_reply_size, 319 .fill_reply = pse_fill_reply, 320 .cleanup_data = pse_cleanup_data, 321 322 .set = ethnl_set_pse, 323 /* PSE has no notification */ 324 }; 325 326 void ethnl_pse_send_ntf(struct net_device *netdev, unsigned long notifs) 327 { 328 void *reply_payload; 329 struct sk_buff *skb; 330 int reply_len; 331 int ret; 332 333 ASSERT_RTNL(); 334 335 if (!netdev || !notifs) 336 return; 337 338 reply_len = ethnl_reply_header_size() + 339 nla_total_size(sizeof(u32)); /* _PSE_NTF_EVENTS */ 340 341 skb = genlmsg_new(reply_len, GFP_KERNEL); 342 if (!skb) 343 return; 344 345 reply_payload = ethnl_bcastmsg_put(skb, ETHTOOL_MSG_PSE_NTF); 346 if (!reply_payload) 347 goto err_skb; 348 349 ret = ethnl_fill_reply_header(skb, netdev, ETHTOOL_A_PSE_NTF_HEADER); 350 if (ret < 0) 351 goto err_skb; 352 353 if (nla_put_uint(skb, ETHTOOL_A_PSE_NTF_EVENTS, notifs)) 354 goto err_skb; 355 356 genlmsg_end(skb, reply_payload); 357 ethnl_multicast(skb, netdev); 358 return; 359 360 err_skb: 361 nlmsg_free(skb); 362 } 363 EXPORT_SYMBOL_GPL(ethnl_pse_send_ntf); 364