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