1*18ff0bcdSOleksij Rempel // SPDX-License-Identifier: GPL-2.0-only 2*18ff0bcdSOleksij Rempel // 3*18ff0bcdSOleksij Rempel // ethtool interface for for Ethernet PSE (Power Sourcing Equipment) 4*18ff0bcdSOleksij Rempel // and PD (Powered Device) 5*18ff0bcdSOleksij Rempel // 6*18ff0bcdSOleksij Rempel // Copyright (c) 2022 Pengutronix, Oleksij Rempel <kernel@pengutronix.de> 7*18ff0bcdSOleksij Rempel // 8*18ff0bcdSOleksij Rempel 9*18ff0bcdSOleksij Rempel #include "common.h" 10*18ff0bcdSOleksij Rempel #include "linux/pse-pd/pse.h" 11*18ff0bcdSOleksij Rempel #include "netlink.h" 12*18ff0bcdSOleksij Rempel #include <linux/ethtool_netlink.h> 13*18ff0bcdSOleksij Rempel #include <linux/ethtool.h> 14*18ff0bcdSOleksij Rempel #include <linux/phy.h> 15*18ff0bcdSOleksij Rempel 16*18ff0bcdSOleksij Rempel struct pse_req_info { 17*18ff0bcdSOleksij Rempel struct ethnl_req_info base; 18*18ff0bcdSOleksij Rempel }; 19*18ff0bcdSOleksij Rempel 20*18ff0bcdSOleksij Rempel struct pse_reply_data { 21*18ff0bcdSOleksij Rempel struct ethnl_reply_data base; 22*18ff0bcdSOleksij Rempel struct pse_control_status status; 23*18ff0bcdSOleksij Rempel }; 24*18ff0bcdSOleksij Rempel 25*18ff0bcdSOleksij Rempel #define PSE_REPDATA(__reply_base) \ 26*18ff0bcdSOleksij Rempel container_of(__reply_base, struct pse_reply_data, base) 27*18ff0bcdSOleksij Rempel 28*18ff0bcdSOleksij Rempel /* PSE_GET */ 29*18ff0bcdSOleksij Rempel 30*18ff0bcdSOleksij Rempel const struct nla_policy ethnl_pse_get_policy[ETHTOOL_A_PSE_HEADER + 1] = { 31*18ff0bcdSOleksij Rempel [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy), 32*18ff0bcdSOleksij Rempel }; 33*18ff0bcdSOleksij Rempel 34*18ff0bcdSOleksij Rempel static int pse_get_pse_attributes(struct net_device *dev, 35*18ff0bcdSOleksij Rempel struct netlink_ext_ack *extack, 36*18ff0bcdSOleksij Rempel struct pse_reply_data *data) 37*18ff0bcdSOleksij Rempel { 38*18ff0bcdSOleksij Rempel struct phy_device *phydev = dev->phydev; 39*18ff0bcdSOleksij Rempel 40*18ff0bcdSOleksij Rempel if (!phydev) { 41*18ff0bcdSOleksij Rempel NL_SET_ERR_MSG(extack, "No PHY is attached"); 42*18ff0bcdSOleksij Rempel return -EOPNOTSUPP; 43*18ff0bcdSOleksij Rempel } 44*18ff0bcdSOleksij Rempel 45*18ff0bcdSOleksij Rempel if (!phydev->psec) { 46*18ff0bcdSOleksij Rempel NL_SET_ERR_MSG(extack, "No PSE is attached"); 47*18ff0bcdSOleksij Rempel return -EOPNOTSUPP; 48*18ff0bcdSOleksij Rempel } 49*18ff0bcdSOleksij Rempel 50*18ff0bcdSOleksij Rempel memset(&data->status, 0, sizeof(data->status)); 51*18ff0bcdSOleksij Rempel 52*18ff0bcdSOleksij Rempel return pse_ethtool_get_status(phydev->psec, extack, &data->status); 53*18ff0bcdSOleksij Rempel } 54*18ff0bcdSOleksij Rempel 55*18ff0bcdSOleksij Rempel static int pse_prepare_data(const struct ethnl_req_info *req_base, 56*18ff0bcdSOleksij Rempel struct ethnl_reply_data *reply_base, 57*18ff0bcdSOleksij Rempel struct genl_info *info) 58*18ff0bcdSOleksij Rempel { 59*18ff0bcdSOleksij Rempel struct pse_reply_data *data = PSE_REPDATA(reply_base); 60*18ff0bcdSOleksij Rempel struct net_device *dev = reply_base->dev; 61*18ff0bcdSOleksij Rempel int ret; 62*18ff0bcdSOleksij Rempel 63*18ff0bcdSOleksij Rempel ret = ethnl_ops_begin(dev); 64*18ff0bcdSOleksij Rempel if (ret < 0) 65*18ff0bcdSOleksij Rempel return ret; 66*18ff0bcdSOleksij Rempel 67*18ff0bcdSOleksij Rempel ret = pse_get_pse_attributes(dev, info->extack, data); 68*18ff0bcdSOleksij Rempel 69*18ff0bcdSOleksij Rempel ethnl_ops_complete(dev); 70*18ff0bcdSOleksij Rempel 71*18ff0bcdSOleksij Rempel return ret; 72*18ff0bcdSOleksij Rempel } 73*18ff0bcdSOleksij Rempel 74*18ff0bcdSOleksij Rempel static int pse_reply_size(const struct ethnl_req_info *req_base, 75*18ff0bcdSOleksij Rempel const struct ethnl_reply_data *reply_base) 76*18ff0bcdSOleksij Rempel { 77*18ff0bcdSOleksij Rempel const struct pse_reply_data *data = PSE_REPDATA(reply_base); 78*18ff0bcdSOleksij Rempel const struct pse_control_status *st = &data->status; 79*18ff0bcdSOleksij Rempel int len = 0; 80*18ff0bcdSOleksij Rempel 81*18ff0bcdSOleksij Rempel if (st->podl_admin_state > 0) 82*18ff0bcdSOleksij Rempel len += nla_total_size(sizeof(u32)); /* _PODL_PSE_ADMIN_STATE */ 83*18ff0bcdSOleksij Rempel if (st->podl_pw_status > 0) 84*18ff0bcdSOleksij Rempel len += nla_total_size(sizeof(u32)); /* _PODL_PSE_PW_D_STATUS */ 85*18ff0bcdSOleksij Rempel 86*18ff0bcdSOleksij Rempel return len; 87*18ff0bcdSOleksij Rempel } 88*18ff0bcdSOleksij Rempel 89*18ff0bcdSOleksij Rempel static int pse_fill_reply(struct sk_buff *skb, 90*18ff0bcdSOleksij Rempel const struct ethnl_req_info *req_base, 91*18ff0bcdSOleksij Rempel const struct ethnl_reply_data *reply_base) 92*18ff0bcdSOleksij Rempel { 93*18ff0bcdSOleksij Rempel const struct pse_reply_data *data = PSE_REPDATA(reply_base); 94*18ff0bcdSOleksij Rempel const struct pse_control_status *st = &data->status; 95*18ff0bcdSOleksij Rempel 96*18ff0bcdSOleksij Rempel if (st->podl_admin_state > 0 && 97*18ff0bcdSOleksij Rempel nla_put_u32(skb, ETHTOOL_A_PODL_PSE_ADMIN_STATE, 98*18ff0bcdSOleksij Rempel st->podl_admin_state)) 99*18ff0bcdSOleksij Rempel return -EMSGSIZE; 100*18ff0bcdSOleksij Rempel 101*18ff0bcdSOleksij Rempel if (st->podl_pw_status > 0 && 102*18ff0bcdSOleksij Rempel nla_put_u32(skb, ETHTOOL_A_PODL_PSE_PW_D_STATUS, 103*18ff0bcdSOleksij Rempel st->podl_pw_status)) 104*18ff0bcdSOleksij Rempel return -EMSGSIZE; 105*18ff0bcdSOleksij Rempel 106*18ff0bcdSOleksij Rempel return 0; 107*18ff0bcdSOleksij Rempel } 108*18ff0bcdSOleksij Rempel 109*18ff0bcdSOleksij Rempel const struct ethnl_request_ops ethnl_pse_request_ops = { 110*18ff0bcdSOleksij Rempel .request_cmd = ETHTOOL_MSG_PSE_GET, 111*18ff0bcdSOleksij Rempel .reply_cmd = ETHTOOL_MSG_PSE_GET_REPLY, 112*18ff0bcdSOleksij Rempel .hdr_attr = ETHTOOL_A_PSE_HEADER, 113*18ff0bcdSOleksij Rempel .req_info_size = sizeof(struct pse_req_info), 114*18ff0bcdSOleksij Rempel .reply_data_size = sizeof(struct pse_reply_data), 115*18ff0bcdSOleksij Rempel 116*18ff0bcdSOleksij Rempel .prepare_data = pse_prepare_data, 117*18ff0bcdSOleksij Rempel .reply_size = pse_reply_size, 118*18ff0bcdSOleksij Rempel .fill_reply = pse_fill_reply, 119*18ff0bcdSOleksij Rempel }; 120*18ff0bcdSOleksij Rempel 121*18ff0bcdSOleksij Rempel /* PSE_SET */ 122*18ff0bcdSOleksij Rempel 123*18ff0bcdSOleksij Rempel const struct nla_policy ethnl_pse_set_policy[ETHTOOL_A_PSE_MAX + 1] = { 124*18ff0bcdSOleksij Rempel [ETHTOOL_A_PSE_HEADER] = NLA_POLICY_NESTED(ethnl_header_policy), 125*18ff0bcdSOleksij Rempel [ETHTOOL_A_PODL_PSE_ADMIN_CONTROL] = 126*18ff0bcdSOleksij Rempel NLA_POLICY_RANGE(NLA_U32, ETHTOOL_PODL_PSE_ADMIN_STATE_DISABLED, 127*18ff0bcdSOleksij Rempel ETHTOOL_PODL_PSE_ADMIN_STATE_ENABLED), 128*18ff0bcdSOleksij Rempel }; 129*18ff0bcdSOleksij Rempel 130*18ff0bcdSOleksij Rempel static int pse_set_pse_config(struct net_device *dev, 131*18ff0bcdSOleksij Rempel struct netlink_ext_ack *extack, 132*18ff0bcdSOleksij Rempel struct nlattr **tb) 133*18ff0bcdSOleksij Rempel { 134*18ff0bcdSOleksij Rempel struct phy_device *phydev = dev->phydev; 135*18ff0bcdSOleksij Rempel struct pse_control_config config = {}; 136*18ff0bcdSOleksij Rempel 137*18ff0bcdSOleksij Rempel /* Optional attribute. Do not return error if not set. */ 138*18ff0bcdSOleksij Rempel if (!tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]) 139*18ff0bcdSOleksij Rempel return 0; 140*18ff0bcdSOleksij Rempel 141*18ff0bcdSOleksij Rempel /* this values are already validated by the ethnl_pse_set_policy */ 142*18ff0bcdSOleksij Rempel config.admin_cotrol = nla_get_u32(tb[ETHTOOL_A_PODL_PSE_ADMIN_CONTROL]); 143*18ff0bcdSOleksij Rempel 144*18ff0bcdSOleksij Rempel if (!phydev) { 145*18ff0bcdSOleksij Rempel NL_SET_ERR_MSG(extack, "No PHY is attached"); 146*18ff0bcdSOleksij Rempel return -EOPNOTSUPP; 147*18ff0bcdSOleksij Rempel } 148*18ff0bcdSOleksij Rempel 149*18ff0bcdSOleksij Rempel if (!phydev->psec) { 150*18ff0bcdSOleksij Rempel NL_SET_ERR_MSG(extack, "No PSE is attached"); 151*18ff0bcdSOleksij Rempel return -EOPNOTSUPP; 152*18ff0bcdSOleksij Rempel } 153*18ff0bcdSOleksij Rempel 154*18ff0bcdSOleksij Rempel return pse_ethtool_set_config(phydev->psec, extack, &config); 155*18ff0bcdSOleksij Rempel } 156*18ff0bcdSOleksij Rempel 157*18ff0bcdSOleksij Rempel int ethnl_set_pse(struct sk_buff *skb, struct genl_info *info) 158*18ff0bcdSOleksij Rempel { 159*18ff0bcdSOleksij Rempel struct ethnl_req_info req_info = {}; 160*18ff0bcdSOleksij Rempel struct nlattr **tb = info->attrs; 161*18ff0bcdSOleksij Rempel struct net_device *dev; 162*18ff0bcdSOleksij Rempel int ret; 163*18ff0bcdSOleksij Rempel 164*18ff0bcdSOleksij Rempel ret = ethnl_parse_header_dev_get(&req_info, tb[ETHTOOL_A_PSE_HEADER], 165*18ff0bcdSOleksij Rempel genl_info_net(info), info->extack, 166*18ff0bcdSOleksij Rempel true); 167*18ff0bcdSOleksij Rempel if (ret < 0) 168*18ff0bcdSOleksij Rempel return ret; 169*18ff0bcdSOleksij Rempel 170*18ff0bcdSOleksij Rempel dev = req_info.dev; 171*18ff0bcdSOleksij Rempel 172*18ff0bcdSOleksij Rempel rtnl_lock(); 173*18ff0bcdSOleksij Rempel ret = ethnl_ops_begin(dev); 174*18ff0bcdSOleksij Rempel if (ret < 0) 175*18ff0bcdSOleksij Rempel goto out_rtnl; 176*18ff0bcdSOleksij Rempel 177*18ff0bcdSOleksij Rempel ret = pse_set_pse_config(dev, info->extack, tb); 178*18ff0bcdSOleksij Rempel ethnl_ops_complete(dev); 179*18ff0bcdSOleksij Rempel out_rtnl: 180*18ff0bcdSOleksij Rempel rtnl_unlock(); 181*18ff0bcdSOleksij Rempel 182*18ff0bcdSOleksij Rempel ethnl_parse_header_dev_put(&req_info); 183*18ff0bcdSOleksij Rempel 184*18ff0bcdSOleksij Rempel return ret; 185*18ff0bcdSOleksij Rempel } 186