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 struct ethtool_link_ext_stats link_stats; 17 bool link_ext_state_provided; 18 struct ethtool_link_ext_state_info ethtool_link_ext_state_info; 19 }; 20 21 #define LINKSTATE_REPDATA(__reply_base) \ 22 container_of(__reply_base, struct linkstate_reply_data, base) 23 24 const struct nla_policy ethnl_linkstate_get_policy[] = { 25 [ETHTOOL_A_LINKSTATE_HEADER] = 26 NLA_POLICY_NESTED(ethnl_header_policy_stats), 27 }; 28 29 static int linkstate_get_sqi(struct net_device *dev) 30 { 31 struct phy_device *phydev = dev->phydev; 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 net_device *dev) 50 { 51 struct phy_device *phydev = dev->phydev; 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 int ret; 104 105 ret = ethnl_ops_begin(dev); 106 if (ret < 0) 107 return ret; 108 data->link = __ethtool_get_link(dev); 109 110 ret = linkstate_get_sqi(dev); 111 if (linkstate_sqi_critical_error(ret)) 112 goto out; 113 data->sqi = ret; 114 115 ret = linkstate_get_sqi_max(dev); 116 if (linkstate_sqi_critical_error(ret)) 117 goto out; 118 data->sqi_max = ret; 119 120 if (dev->flags & IFF_UP) { 121 ret = linkstate_get_link_ext_state(dev, data); 122 if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA) 123 goto out; 124 } 125 126 ethtool_stats_init((u64 *)&data->link_stats, 127 sizeof(data->link_stats) / 8); 128 129 if (req_base->flags & ETHTOOL_FLAG_STATS) { 130 if (dev->phydev) 131 data->link_stats.link_down_events = 132 READ_ONCE(dev->phydev->link_down_events); 133 134 if (dev->ethtool_ops->get_link_ext_stats) 135 dev->ethtool_ops->get_link_ext_stats(dev, 136 &data->link_stats); 137 } 138 139 ret = 0; 140 out: 141 ethnl_ops_complete(dev); 142 return ret; 143 } 144 145 static int linkstate_reply_size(const struct ethnl_req_info *req_base, 146 const struct ethnl_reply_data *reply_base) 147 { 148 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 149 int len; 150 151 len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */ 152 + 0; 153 154 if (linkstate_sqi_valid(data)) { 155 len += nla_total_size(sizeof(u32)); /* LINKSTATE_SQI */ 156 len += nla_total_size(sizeof(u32)); /* LINKSTATE_SQI_MAX */ 157 } 158 159 if (data->link_ext_state_provided) 160 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */ 161 162 if (data->ethtool_link_ext_state_info.__link_ext_substate) 163 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */ 164 165 if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET) 166 len += nla_total_size(sizeof(u32)); 167 168 return len; 169 } 170 171 static int linkstate_fill_reply(struct sk_buff *skb, 172 const struct ethnl_req_info *req_base, 173 const struct ethnl_reply_data *reply_base) 174 { 175 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 176 177 if (data->link >= 0 && 178 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link)) 179 return -EMSGSIZE; 180 181 if (linkstate_sqi_valid(data)) { 182 if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi)) 183 return -EMSGSIZE; 184 185 if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX, 186 data->sqi_max)) 187 return -EMSGSIZE; 188 } 189 190 if (data->link_ext_state_provided) { 191 if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE, 192 data->ethtool_link_ext_state_info.link_ext_state)) 193 return -EMSGSIZE; 194 195 if (data->ethtool_link_ext_state_info.__link_ext_substate && 196 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE, 197 data->ethtool_link_ext_state_info.__link_ext_substate)) 198 return -EMSGSIZE; 199 } 200 201 if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET) 202 if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_EXT_DOWN_CNT, 203 data->link_stats.link_down_events)) 204 return -EMSGSIZE; 205 206 return 0; 207 } 208 209 const struct ethnl_request_ops ethnl_linkstate_request_ops = { 210 .request_cmd = ETHTOOL_MSG_LINKSTATE_GET, 211 .reply_cmd = ETHTOOL_MSG_LINKSTATE_GET_REPLY, 212 .hdr_attr = ETHTOOL_A_LINKSTATE_HEADER, 213 .req_info_size = sizeof(struct linkstate_req_info), 214 .reply_data_size = sizeof(struct linkstate_reply_data), 215 216 .prepare_data = linkstate_prepare_data, 217 .reply_size = linkstate_reply_size, 218 .fill_reply = linkstate_fill_reply, 219 }; 220