1 // SPDX-License-Identifier: GPL-2.0-only 2 3 #include "netlink.h" 4 #include "common.h" 5 #include <linux/phy.h> 6 7 struct linkstate_req_info { 8 struct ethnl_req_info base; 9 }; 10 11 struct linkstate_reply_data { 12 struct ethnl_reply_data base; 13 int link; 14 int sqi; 15 int sqi_max; 16 bool link_ext_state_provided; 17 struct ethtool_link_ext_state_info ethtool_link_ext_state_info; 18 }; 19 20 #define LINKSTATE_REPDATA(__reply_base) \ 21 container_of(__reply_base, struct linkstate_reply_data, base) 22 23 const struct nla_policy ethnl_linkstate_get_policy[] = { 24 [ETHTOOL_A_LINKSTATE_HEADER] = 25 NLA_POLICY_NESTED(ethnl_header_policy), 26 }; 27 28 static int linkstate_get_sqi(struct net_device *dev) 29 { 30 struct phy_device *phydev = dev->phydev; 31 int ret; 32 33 if (!phydev) 34 return -EOPNOTSUPP; 35 36 mutex_lock(&phydev->lock); 37 if (!phydev->drv || !phydev->drv->get_sqi) 38 ret = -EOPNOTSUPP; 39 else 40 ret = phydev->drv->get_sqi(phydev); 41 mutex_unlock(&phydev->lock); 42 43 return ret; 44 } 45 46 static int linkstate_get_sqi_max(struct net_device *dev) 47 { 48 struct phy_device *phydev = dev->phydev; 49 int ret; 50 51 if (!phydev) 52 return -EOPNOTSUPP; 53 54 mutex_lock(&phydev->lock); 55 if (!phydev->drv || !phydev->drv->get_sqi_max) 56 ret = -EOPNOTSUPP; 57 else 58 ret = phydev->drv->get_sqi_max(phydev); 59 mutex_unlock(&phydev->lock); 60 61 return ret; 62 }; 63 64 static int linkstate_get_link_ext_state(struct net_device *dev, 65 struct linkstate_reply_data *data) 66 { 67 int err; 68 69 if (!dev->ethtool_ops->get_link_ext_state) 70 return -EOPNOTSUPP; 71 72 err = dev->ethtool_ops->get_link_ext_state(dev, &data->ethtool_link_ext_state_info); 73 if (err) 74 return err; 75 76 data->link_ext_state_provided = true; 77 78 return 0; 79 } 80 81 static int linkstate_prepare_data(const struct ethnl_req_info *req_base, 82 struct ethnl_reply_data *reply_base, 83 struct genl_info *info) 84 { 85 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 86 struct net_device *dev = reply_base->dev; 87 int ret; 88 89 ret = ethnl_ops_begin(dev); 90 if (ret < 0) 91 return ret; 92 data->link = __ethtool_get_link(dev); 93 94 ret = linkstate_get_sqi(dev); 95 if (ret < 0 && ret != -EOPNOTSUPP) 96 goto out; 97 data->sqi = ret; 98 99 ret = linkstate_get_sqi_max(dev); 100 if (ret < 0 && ret != -EOPNOTSUPP) 101 goto out; 102 data->sqi_max = ret; 103 104 if (dev->flags & IFF_UP) { 105 ret = linkstate_get_link_ext_state(dev, data); 106 if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA) 107 goto out; 108 } 109 110 ret = 0; 111 out: 112 ethnl_ops_complete(dev); 113 return ret; 114 } 115 116 static int linkstate_reply_size(const struct ethnl_req_info *req_base, 117 const struct ethnl_reply_data *reply_base) 118 { 119 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 120 int len; 121 122 len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */ 123 + 0; 124 125 if (data->sqi != -EOPNOTSUPP) 126 len += nla_total_size(sizeof(u32)); 127 128 if (data->sqi_max != -EOPNOTSUPP) 129 len += nla_total_size(sizeof(u32)); 130 131 if (data->link_ext_state_provided) 132 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */ 133 134 if (data->ethtool_link_ext_state_info.__link_ext_substate) 135 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */ 136 137 return len; 138 } 139 140 static int linkstate_fill_reply(struct sk_buff *skb, 141 const struct ethnl_req_info *req_base, 142 const struct ethnl_reply_data *reply_base) 143 { 144 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 145 146 if (data->link >= 0 && 147 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link)) 148 return -EMSGSIZE; 149 150 if (data->sqi != -EOPNOTSUPP && 151 nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi)) 152 return -EMSGSIZE; 153 154 if (data->sqi_max != -EOPNOTSUPP && 155 nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX, data->sqi_max)) 156 return -EMSGSIZE; 157 158 if (data->link_ext_state_provided) { 159 if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE, 160 data->ethtool_link_ext_state_info.link_ext_state)) 161 return -EMSGSIZE; 162 163 if (data->ethtool_link_ext_state_info.__link_ext_substate && 164 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE, 165 data->ethtool_link_ext_state_info.__link_ext_substate)) 166 return -EMSGSIZE; 167 } 168 169 return 0; 170 } 171 172 const struct ethnl_request_ops ethnl_linkstate_request_ops = { 173 .request_cmd = ETHTOOL_MSG_LINKSTATE_GET, 174 .reply_cmd = ETHTOOL_MSG_LINKSTATE_GET_REPLY, 175 .hdr_attr = ETHTOOL_A_LINKSTATE_HEADER, 176 .req_info_size = sizeof(struct linkstate_req_info), 177 .reply_data_size = sizeof(struct linkstate_reply_data), 178 179 .prepare_data = linkstate_prepare_data, 180 .reply_size = linkstate_reply_size, 181 .fill_reply = linkstate_fill_reply, 182 }; 183