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 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 pse_control_status *st = &data->status; 84 int len = 0; 85 86 if (st->podl_admin_state > 0) 87 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_ADMIN_STATE */ 88 if (st->podl_pw_status > 0) 89 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_PW_D_STATUS */ 90 if (st->c33_admin_state > 0) 91 len += nla_total_size(sizeof(u32)); /* _C33_PSE_ADMIN_STATE */ 92 if (st->c33_pw_status > 0) 93 len += nla_total_size(sizeof(u32)); /* _C33_PSE_PW_D_STATUS */ 94 if (st->c33_pw_class > 0) 95 len += nla_total_size(sizeof(u32)); /* _C33_PSE_PW_CLASS */ 96 if (st->c33_actual_pw > 0) 97 len += nla_total_size(sizeof(u32)); /* _C33_PSE_ACTUAL_PW */ 98 if (st->c33_ext_state_info.c33_pse_ext_state > 0) { 99 len += nla_total_size(sizeof(u32)); /* _C33_PSE_EXT_STATE */ 100 if (st->c33_ext_state_info.__c33_pse_ext_substate > 0) 101 /* _C33_PSE_EXT_SUBSTATE */ 102 len += nla_total_size(sizeof(u32)); 103 } 104 if (st->c33_avail_pw_limit > 0) 105 /* _C33_AVAIL_PSE_PW_LIMIT */ 106 len += nla_total_size(sizeof(u32)); 107 if (st->c33_pw_limit_nb_ranges > 0) 108 /* _C33_PSE_PW_LIMIT_RANGES */ 109 len += st->c33_pw_limit_nb_ranges * 110 (nla_total_size(0) + 111 nla_total_size(sizeof(u32)) * 2); 112 113 return len; 114 } 115 116 static int pse_put_pw_limit_ranges(struct sk_buff *skb, 117 const struct pse_control_status *st) 118 { 119 const struct ethtool_c33_pse_pw_limit_range *pw_limit_ranges; 120 int i; 121 122 pw_limit_ranges = st->c33_pw_limit_ranges; 123 for (i = 0; i < st->c33_pw_limit_nb_ranges; i++) { 124 struct nlattr *nest; 125 126 nest = nla_nest_start(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_RANGES); 127 if (!nest) 128 return -EMSGSIZE; 129 130 if (nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_MIN, 131 pw_limit_ranges->min) || 132 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_LIMIT_MAX, 133 pw_limit_ranges->max)) { 134 nla_nest_cancel(skb, nest); 135 return -EMSGSIZE; 136 } 137 nla_nest_end(skb, nest); 138 pw_limit_ranges++; 139 } 140 141 return 0; 142 } 143 144 static int pse_fill_reply(struct sk_buff *skb, 145 const struct ethnl_req_info *req_base, 146 const struct ethnl_reply_data *reply_base) 147 { 148 const struct pse_reply_data *data = PSE_REPDATA(reply_base); 149 const struct pse_control_status *st = &data->status; 150 151 if (st->podl_admin_state > 0 && 152 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_ADMIN_STATE, 153 st->podl_admin_state)) 154 return -EMSGSIZE; 155 156 if (st->podl_pw_status > 0 && 157 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_PW_D_STATUS, 158 st->podl_pw_status)) 159 return -EMSGSIZE; 160 161 if (st->c33_admin_state > 0 && 162 nla_put_u32(skb, ETHTOOL_A_C33_PSE_ADMIN_STATE, 163 st->c33_admin_state)) 164 return -EMSGSIZE; 165 166 if (st->c33_pw_status > 0 && 167 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_D_STATUS, 168 st->c33_pw_status)) 169 return -EMSGSIZE; 170 171 if (st->c33_pw_class > 0 && 172 nla_put_u32(skb, ETHTOOL_A_C33_PSE_PW_CLASS, 173 st->c33_pw_class)) 174 return -EMSGSIZE; 175 176 if (st->c33_actual_pw > 0 && 177 nla_put_u32(skb, ETHTOOL_A_C33_PSE_ACTUAL_PW, 178 st->c33_actual_pw)) 179 return -EMSGSIZE; 180 181 if (st->c33_ext_state_info.c33_pse_ext_state > 0) { 182 if (nla_put_u32(skb, ETHTOOL_A_C33_PSE_EXT_STATE, 183 st->c33_ext_state_info.c33_pse_ext_state)) 184 return -EMSGSIZE; 185 186 if (st->c33_ext_state_info.__c33_pse_ext_substate > 0 && 187 nla_put_u32(skb, ETHTOOL_A_C33_PSE_EXT_SUBSTATE, 188 st->c33_ext_state_info.__c33_pse_ext_substate)) 189 return -EMSGSIZE; 190 } 191 192 if (st->c33_avail_pw_limit > 0 && 193 nla_put_u32(skb, ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT, 194 st->c33_avail_pw_limit)) 195 return -EMSGSIZE; 196 197 if (st->c33_pw_limit_nb_ranges > 0 && 198 pse_put_pw_limit_ranges(skb, st)) 199 return -EMSGSIZE; 200 201 return 0; 202 } 203 204 static void pse_cleanup_data(struct ethnl_reply_data *reply_base) 205 { 206 const struct pse_reply_data *data = PSE_REPDATA(reply_base); 207 208 kfree(data->status.c33_pw_limit_ranges); 209 } 210 211 /* PSE_SET */ 212 213 const struct nla_policy ethnl_pse_set_policy[ETHTOOL_A_PSE_MAX + 1] = { 214 [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy_phy), 215 [ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] = 216 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED, 217 ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED), 218 [ETHTOOL_A_C33_PSE_ADMIN_CONTROL] = 219 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_C33_PSE_ADMIN_STATE_DISABLED, 220 ETHTOOL_C33_PSE_ADMIN_STATE_ENABLED), 221 [ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT] = { .type = NLA_U32 }, 222 }; 223 224 static int 225 ethnl_set_pse_validate(struct phy_device *phydev, struct genl_info *info) 226 { 227 struct nlattr **tb = info->attrs; 228 229 if (IS_ERR_OR_NULL(phydev)) { 230 NL_SET_ERR_MSG(info->extack, "No PHY is attached"); 231 return -EOPNOTSUPP; 232 } 233 234 if (!phydev->psec) { 235 NL_SET_ERR_MSG(info->extack, "No PSE is attached"); 236 return -EOPNOTSUPP; 237 } 238 239 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] && 240 !pse_has_podl(phydev->psec)) { 241 NL_SET_ERR_MSG_ATTR(info->extack, 242 tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL], 243 "setting PoDL PSE admin control not supported"); 244 return -EOPNOTSUPP; 245 } 246 if (tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL] && 247 !pse_has_c33(phydev->psec)) { 248 NL_SET_ERR_MSG_ATTR(info->extack, 249 tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL], 250 "setting C33 PSE admin control not supported"); 251 return -EOPNOTSUPP; 252 } 253 254 return 0; 255 } 256 257 static int 258 ethnl_set_pse(struct ethnl_req_info *req_info, struct genl_info *info) 259 { 260 struct nlattr **tb = info->attrs; 261 struct phy_device *phydev; 262 int ret; 263 264 phydev = ethnl_req_get_phydev(req_info, tb[ETHTOOL_A_PSE_HEADER], 265 info->extack); 266 ret = ethnl_set_pse_validate(phydev, info); 267 if (ret) 268 return ret; 269 270 if (tb[ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT]) { 271 unsigned int pw_limit; 272 273 pw_limit = nla_get_u32(tb[ETHTOOL_A_C33_PSE_AVAIL_PW_LIMIT]); 274 ret = pse_ethtool_set_pw_limit(phydev->psec, info->extack, 275 pw_limit); 276 if (ret) 277 return ret; 278 } 279 280 /* These values are already validated by the ethnl_pse_set_policy */ 281 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] || 282 tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]) { 283 struct pse_control_config config = {}; 284 285 if (tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]) 286 config.podl_admin_control = nla_get_u32(tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]); 287 if (tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]) 288 config.c33_admin_control = nla_get_u32(tb[ETHTOOL_A_C33_PSE_ADMIN_CONTROL]); 289 290 /* pse_ethtool_set_config() will do nothing if the config 291 * is zero 292 */ 293 ret = pse_ethtool_set_config(phydev->psec, info->extack, 294 &config); 295 if (ret) 296 return ret; 297 } 298 299 /* Return errno or zero - PSE has no notification */ 300 return ret; 301 } 302 303 const struct ethnl_request_ops ethnl_pse_request_ops = { 304 .request_cmd = ETHTOOL_MSG_PSE_GET, 305 .reply_cmd = ETHTOOL_MSG_PSE_GET_REPLY, 306 .hdr_attr = ETHTOOL_A_PSE_HEADER, 307 .req_info_size = sizeof(struct pse_req_info), 308 .reply_data_size = sizeof(struct pse_reply_data), 309 310 .prepare_data = pse_prepare_data, 311 .reply_size = pse_reply_size, 312 .fill_reply = pse_fill_reply, 313 .cleanup_data = pse_cleanup_data, 314 315 .set = ethnl_set_pse, 316 /* PSE has no notification */ 317 }; 318