13d2b847fSMichal Kubecek // SPDX-License-Identifier: GPL-2.0-only 23d2b847fSMichal Kubecek 33d2b847fSMichal Kubecek #include "netlink.h" 43d2b847fSMichal Kubecek #include "common.h" 580660219SOleksij Rempel #include <linux/phy.h> 63d2b847fSMichal Kubecek 73d2b847fSMichal Kubecek struct linkstate_req_info { 83d2b847fSMichal Kubecek struct ethnl_req_info base; 93d2b847fSMichal Kubecek }; 103d2b847fSMichal Kubecek 113d2b847fSMichal Kubecek struct linkstate_reply_data { 123d2b847fSMichal Kubecek struct ethnl_reply_data base; 133d2b847fSMichal Kubecek int link; 1480660219SOleksij Rempel int sqi; 1580660219SOleksij Rempel int sqi_max; 16ecc31c60SAmit Cohen bool link_ext_state_provided; 17ecc31c60SAmit Cohen struct ethtool_link_ext_state_info ethtool_link_ext_state_info; 183d2b847fSMichal Kubecek }; 193d2b847fSMichal Kubecek 203d2b847fSMichal Kubecek #define LINKSTATE_REPDATA(__reply_base) \ 213d2b847fSMichal Kubecek container_of(__reply_base, struct linkstate_reply_data, base) 223d2b847fSMichal Kubecek 23ff419afaSJakub Kicinski const struct nla_policy ethnl_linkstate_get_policy[] = { 243d2b847fSMichal Kubecek [ETHTOOL_A_LINKSTATE_HEADER] = { .type = NLA_NESTED }, 253d2b847fSMichal Kubecek }; 263d2b847fSMichal Kubecek 2780660219SOleksij Rempel static int linkstate_get_sqi(struct net_device *dev) 2880660219SOleksij Rempel { 2980660219SOleksij Rempel struct phy_device *phydev = dev->phydev; 3080660219SOleksij Rempel int ret; 3180660219SOleksij Rempel 3280660219SOleksij Rempel if (!phydev) 3380660219SOleksij Rempel return -EOPNOTSUPP; 3480660219SOleksij Rempel 3580660219SOleksij Rempel mutex_lock(&phydev->lock); 3680660219SOleksij Rempel if (!phydev->drv || !phydev->drv->get_sqi) 3780660219SOleksij Rempel ret = -EOPNOTSUPP; 3880660219SOleksij Rempel else 3980660219SOleksij Rempel ret = phydev->drv->get_sqi(phydev); 4080660219SOleksij Rempel mutex_unlock(&phydev->lock); 4180660219SOleksij Rempel 4280660219SOleksij Rempel return ret; 4380660219SOleksij Rempel } 4480660219SOleksij Rempel 4580660219SOleksij Rempel static int linkstate_get_sqi_max(struct net_device *dev) 4680660219SOleksij Rempel { 4780660219SOleksij Rempel struct phy_device *phydev = dev->phydev; 4880660219SOleksij Rempel int ret; 4980660219SOleksij Rempel 5080660219SOleksij Rempel if (!phydev) 5180660219SOleksij Rempel return -EOPNOTSUPP; 5280660219SOleksij Rempel 5380660219SOleksij Rempel mutex_lock(&phydev->lock); 5480660219SOleksij Rempel if (!phydev->drv || !phydev->drv->get_sqi_max) 5580660219SOleksij Rempel ret = -EOPNOTSUPP; 5680660219SOleksij Rempel else 5780660219SOleksij Rempel ret = phydev->drv->get_sqi_max(phydev); 5880660219SOleksij Rempel mutex_unlock(&phydev->lock); 5980660219SOleksij Rempel 6080660219SOleksij Rempel return ret; 61ecc31c60SAmit Cohen }; 62ecc31c60SAmit Cohen 63ecc31c60SAmit Cohen static int linkstate_get_link_ext_state(struct net_device *dev, 64ecc31c60SAmit Cohen struct linkstate_reply_data *data) 65ecc31c60SAmit Cohen { 66ecc31c60SAmit Cohen int err; 67ecc31c60SAmit Cohen 68ecc31c60SAmit Cohen if (!dev->ethtool_ops->get_link_ext_state) 69ecc31c60SAmit Cohen return -EOPNOTSUPP; 70ecc31c60SAmit Cohen 71ecc31c60SAmit Cohen err = dev->ethtool_ops->get_link_ext_state(dev, &data->ethtool_link_ext_state_info); 72ecc31c60SAmit Cohen if (err) 73ecc31c60SAmit Cohen return err; 74ecc31c60SAmit Cohen 75ecc31c60SAmit Cohen data->link_ext_state_provided = true; 76ecc31c60SAmit Cohen 77ecc31c60SAmit Cohen return 0; 7880660219SOleksij Rempel } 7980660219SOleksij Rempel 803d2b847fSMichal Kubecek static int linkstate_prepare_data(const struct ethnl_req_info *req_base, 813d2b847fSMichal Kubecek struct ethnl_reply_data *reply_base, 823d2b847fSMichal Kubecek struct genl_info *info) 833d2b847fSMichal Kubecek { 843d2b847fSMichal Kubecek struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 853d2b847fSMichal Kubecek struct net_device *dev = reply_base->dev; 863d2b847fSMichal Kubecek int ret; 873d2b847fSMichal Kubecek 883d2b847fSMichal Kubecek ret = ethnl_ops_begin(dev); 893d2b847fSMichal Kubecek if (ret < 0) 903d2b847fSMichal Kubecek return ret; 913d2b847fSMichal Kubecek data->link = __ethtool_get_link(dev); 9280660219SOleksij Rempel 9380660219SOleksij Rempel ret = linkstate_get_sqi(dev); 9480660219SOleksij Rempel if (ret < 0 && ret != -EOPNOTSUPP) 951ae71d99SMichal Kubecek goto out; 9680660219SOleksij Rempel data->sqi = ret; 9780660219SOleksij Rempel 9880660219SOleksij Rempel ret = linkstate_get_sqi_max(dev); 9980660219SOleksij Rempel if (ret < 0 && ret != -EOPNOTSUPP) 1001ae71d99SMichal Kubecek goto out; 10180660219SOleksij Rempel data->sqi_max = ret; 10280660219SOleksij Rempel 103ecc31c60SAmit Cohen if (dev->flags & IFF_UP) { 104ecc31c60SAmit Cohen ret = linkstate_get_link_ext_state(dev, data); 105ecc31c60SAmit Cohen if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA) 106ecc31c60SAmit Cohen goto out; 107ecc31c60SAmit Cohen } 108ecc31c60SAmit Cohen 1091ae71d99SMichal Kubecek ret = 0; 1101ae71d99SMichal Kubecek out: 1113d2b847fSMichal Kubecek ethnl_ops_complete(dev); 1121ae71d99SMichal Kubecek return ret; 1133d2b847fSMichal Kubecek } 1143d2b847fSMichal Kubecek 1153d2b847fSMichal Kubecek static int linkstate_reply_size(const struct ethnl_req_info *req_base, 1163d2b847fSMichal Kubecek const struct ethnl_reply_data *reply_base) 1173d2b847fSMichal Kubecek { 11880660219SOleksij Rempel struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 11980660219SOleksij Rempel int len; 12080660219SOleksij Rempel 12180660219SOleksij Rempel len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */ 1223d2b847fSMichal Kubecek + 0; 12380660219SOleksij Rempel 12480660219SOleksij Rempel if (data->sqi != -EOPNOTSUPP) 12580660219SOleksij Rempel len += nla_total_size(sizeof(u32)); 12680660219SOleksij Rempel 12780660219SOleksij Rempel if (data->sqi_max != -EOPNOTSUPP) 12880660219SOleksij Rempel len += nla_total_size(sizeof(u32)); 12980660219SOleksij Rempel 130ecc31c60SAmit Cohen if (data->link_ext_state_provided) 131ecc31c60SAmit Cohen len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */ 132ecc31c60SAmit Cohen 133ecc31c60SAmit Cohen if (data->ethtool_link_ext_state_info.__link_ext_substate) 134ecc31c60SAmit Cohen len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */ 135ecc31c60SAmit Cohen 13680660219SOleksij Rempel return len; 1373d2b847fSMichal Kubecek } 1383d2b847fSMichal Kubecek 1393d2b847fSMichal Kubecek static int linkstate_fill_reply(struct sk_buff *skb, 1403d2b847fSMichal Kubecek const struct ethnl_req_info *req_base, 1413d2b847fSMichal Kubecek const struct ethnl_reply_data *reply_base) 1423d2b847fSMichal Kubecek { 1433d2b847fSMichal Kubecek struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 1443d2b847fSMichal Kubecek 1453d2b847fSMichal Kubecek if (data->link >= 0 && 1463d2b847fSMichal Kubecek nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link)) 1473d2b847fSMichal Kubecek return -EMSGSIZE; 1483d2b847fSMichal Kubecek 14980660219SOleksij Rempel if (data->sqi != -EOPNOTSUPP && 15080660219SOleksij Rempel nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi)) 15180660219SOleksij Rempel return -EMSGSIZE; 15280660219SOleksij Rempel 15380660219SOleksij Rempel if (data->sqi_max != -EOPNOTSUPP && 15480660219SOleksij Rempel nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX, data->sqi_max)) 15580660219SOleksij Rempel return -EMSGSIZE; 15680660219SOleksij Rempel 157ecc31c60SAmit Cohen if (data->link_ext_state_provided) { 158ecc31c60SAmit Cohen if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE, 159ecc31c60SAmit Cohen data->ethtool_link_ext_state_info.link_ext_state)) 160ecc31c60SAmit Cohen return -EMSGSIZE; 161ecc31c60SAmit Cohen 162ecc31c60SAmit Cohen if (data->ethtool_link_ext_state_info.__link_ext_substate && 163ecc31c60SAmit Cohen nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE, 164ecc31c60SAmit Cohen data->ethtool_link_ext_state_info.__link_ext_substate)) 165ecc31c60SAmit Cohen return -EMSGSIZE; 166ecc31c60SAmit Cohen } 167ecc31c60SAmit Cohen 1683d2b847fSMichal Kubecek return 0; 1693d2b847fSMichal Kubecek } 1703d2b847fSMichal Kubecek 1713d2b847fSMichal Kubecek const struct ethnl_request_ops ethnl_linkstate_request_ops = { 1723d2b847fSMichal Kubecek .request_cmd = ETHTOOL_MSG_LINKSTATE_GET, 1733d2b847fSMichal Kubecek .reply_cmd = ETHTOOL_MSG_LINKSTATE_GET_REPLY, 1743d2b847fSMichal Kubecek .hdr_attr = ETHTOOL_A_LINKSTATE_HEADER, 1753d2b847fSMichal Kubecek .req_info_size = sizeof(struct linkstate_req_info), 1763d2b847fSMichal Kubecek .reply_data_size = sizeof(struct linkstate_reply_data), 1773d2b847fSMichal Kubecek 1783d2b847fSMichal Kubecek .prepare_data = linkstate_prepare_data, 1793d2b847fSMichal Kubecek .reply_size = linkstate_reply_size, 1803d2b847fSMichal Kubecek .fill_reply = linkstate_fill_reply, 1813d2b847fSMichal Kubecek }; 182