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 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 is attached"); 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 int ret; 60 61 ret = ethnl_ops_begin(dev); 62 if (ret < 0) 63 return ret; 64 65 ret = pse_get_pse_attributes(req_base->phydev, info->extack, data); 66 67 ethnl_ops_complete(dev); 68 69 return ret; 70 } 71 72 static int pse_reply_size(const struct ethnl_req_info *req_base, 73 const struct ethnl_reply_data *reply_base) 74 { 75 const struct pse_reply_data *data = PSE_REPDATA(reply_base); 76 const struct pse_control_status *st = &data->status; 77 int len = 0; 78 79 if (st->podl_admin_state > 0) 80 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_ADMIN_STATE */ 81 if (st->podl_pw_status > 0) 82 len += nla_total_size(sizeof(u32)); /* _PODL_PSE_PW_D_STATUS */ 83 84 return len; 85 } 86 87 static int pse_fill_reply(struct sk_buff *skb, 88 const struct ethnl_req_info *req_base, 89 const struct ethnl_reply_data *reply_base) 90 { 91 const struct pse_reply_data *data = PSE_REPDATA(reply_base); 92 const struct pse_control_status *st = &data->status; 93 94 if (st->podl_admin_state > 0 && 95 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_ADMIN_STATE, 96 st->podl_admin_state)) 97 return -EMSGSIZE; 98 99 if (st->podl_pw_status > 0 && 100 nla_put_u32(skb, ETHTOOL_A_PODL_PSE_PW_D_STATUS, 101 st->podl_pw_status)) 102 return -EMSGSIZE; 103 104 return 0; 105 } 106 107 /* PSE_SET */ 108 109 const struct nla_policy ethnl_pse_set_policy[ETHTOOL_A_PSE_MAX + 1] = { 110 [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy), 111 [ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] = 112 NLA_POLICY_RANGE(NLA_U32, ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED, 113 ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED), 114 }; 115 116 static int 117 ethnl_set_pse_validate(struct ethnl_req_info *req_info, struct genl_info *info) 118 { 119 return !!info->attrs[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]; 120 } 121 122 static int 123 ethnl_set_pse(struct ethnl_req_info *req_info, struct genl_info *info) 124 { 125 struct pse_control_config config = {}; 126 struct nlattr **tb = info->attrs; 127 struct phy_device *phydev; 128 129 /* this values are already validated by the ethnl_pse_set_policy */ 130 config.admin_cotrol = nla_get_u32(tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]); 131 132 phydev = req_info->phydev; 133 if (!phydev) { 134 NL_SET_ERR_MSG(info->extack, "No PHY is attached"); 135 return -EOPNOTSUPP; 136 } 137 138 if (!phydev->psec) { 139 NL_SET_ERR_MSG(info->extack, "No PSE is attached"); 140 return -EOPNOTSUPP; 141 } 142 143 /* Return errno directly - PSE has no notification */ 144 return pse_ethtool_set_config(phydev->psec, info->extack, &config); 145 } 146 147 const struct ethnl_request_ops ethnl_pse_request_ops = { 148 .request_cmd = ETHTOOL_MSG_PSE_GET, 149 .reply_cmd = ETHTOOL_MSG_PSE_GET_REPLY, 150 .hdr_attr = ETHTOOL_A_PSE_HEADER, 151 .req_info_size = sizeof(struct pse_req_info), 152 .reply_data_size = sizeof(struct pse_reply_data), 153 154 .prepare_data = pse_prepare_data, 155 .reply_size = pse_reply_size, 156 .fill_reply = pse_fill_reply, 157 158 .set_validate = ethnl_set_pse_validate, 159 .set = ethnl_set_pse, 160 /* PSE has no notification */ 161 }; 162