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