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