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 41 ret = phydev->drv->get_sqi(phydev); 42 mutex_unlock(&phydev->lock); 43 44 return ret; 45 } 46 47 static int linkstate_get_sqi_max(struct net_device *dev) 48 { 49 struct phy_device *phydev = dev->phydev; 50 int ret; 51 52 if (!phydev) 53 return -EOPNOTSUPP; 54 55 mutex_lock(&phydev->lock); 56 if (!phydev->drv || !phydev->drv->get_sqi_max) 57 ret = -EOPNOTSUPP; 58 else 59 ret = phydev->drv->get_sqi_max(phydev); 60 mutex_unlock(&phydev->lock); 61 62 return ret; 63 }; 64 65 static int linkstate_get_link_ext_state(struct net_device *dev, 66 struct linkstate_reply_data *data) 67 { 68 int err; 69 70 if (!dev->ethtool_ops->get_link_ext_state) 71 return -EOPNOTSUPP; 72 73 err = dev->ethtool_ops->get_link_ext_state(dev, &data->ethtool_link_ext_state_info); 74 if (err) 75 return err; 76 77 data->link_ext_state_provided = true; 78 79 return 0; 80 } 81 82 static int linkstate_prepare_data(const struct ethnl_req_info *req_base, 83 struct ethnl_reply_data *reply_base, 84 const struct genl_info *info) 85 { 86 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 87 struct net_device *dev = reply_base->dev; 88 int ret; 89 90 ret = ethnl_ops_begin(dev); 91 if (ret < 0) 92 return ret; 93 data->link = __ethtool_get_link(dev); 94 95 ret = linkstate_get_sqi(dev); 96 if (ret < 0 && ret != -EOPNOTSUPP) 97 goto out; 98 data->sqi = ret; 99 100 ret = linkstate_get_sqi_max(dev); 101 if (ret < 0 && ret != -EOPNOTSUPP) 102 goto out; 103 data->sqi_max = ret; 104 105 if (dev->flags & IFF_UP) { 106 ret = linkstate_get_link_ext_state(dev, data); 107 if (ret < 0 && ret != -EOPNOTSUPP && ret != -ENODATA) 108 goto out; 109 } 110 111 ethtool_stats_init((u64 *)&data->link_stats, 112 sizeof(data->link_stats) / 8); 113 114 if (req_base->flags & ETHTOOL_FLAG_STATS) { 115 if (dev->phydev) 116 data->link_stats.link_down_events = 117 READ_ONCE(dev->phydev->link_down_events); 118 119 if (dev->ethtool_ops->get_link_ext_stats) 120 dev->ethtool_ops->get_link_ext_stats(dev, 121 &data->link_stats); 122 } 123 124 ret = 0; 125 out: 126 ethnl_ops_complete(dev); 127 return ret; 128 } 129 130 static int linkstate_reply_size(const struct ethnl_req_info *req_base, 131 const struct ethnl_reply_data *reply_base) 132 { 133 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 134 int len; 135 136 len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */ 137 + 0; 138 139 if (data->sqi != -EOPNOTSUPP) 140 len += nla_total_size(sizeof(u32)); 141 142 if (data->sqi_max != -EOPNOTSUPP) 143 len += nla_total_size(sizeof(u32)); 144 145 if (data->link_ext_state_provided) 146 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_STATE */ 147 148 if (data->ethtool_link_ext_state_info.__link_ext_substate) 149 len += nla_total_size(sizeof(u8)); /* LINKSTATE_EXT_SUBSTATE */ 150 151 if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET) 152 len += nla_total_size(sizeof(u32)); 153 154 return len; 155 } 156 157 static int linkstate_fill_reply(struct sk_buff *skb, 158 const struct ethnl_req_info *req_base, 159 const struct ethnl_reply_data *reply_base) 160 { 161 struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base); 162 163 if (data->link >= 0 && 164 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link)) 165 return -EMSGSIZE; 166 167 if (data->sqi != -EOPNOTSUPP && 168 nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi)) 169 return -EMSGSIZE; 170 171 if (data->sqi_max != -EOPNOTSUPP && 172 nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX, data->sqi_max)) 173 return -EMSGSIZE; 174 175 if (data->link_ext_state_provided) { 176 if (nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_STATE, 177 data->ethtool_link_ext_state_info.link_ext_state)) 178 return -EMSGSIZE; 179 180 if (data->ethtool_link_ext_state_info.__link_ext_substate && 181 nla_put_u8(skb, ETHTOOL_A_LINKSTATE_EXT_SUBSTATE, 182 data->ethtool_link_ext_state_info.__link_ext_substate)) 183 return -EMSGSIZE; 184 } 185 186 if (data->link_stats.link_down_events != ETHTOOL_STAT_NOT_SET) 187 if (nla_put_u32(skb, ETHTOOL_A_LINKSTATE_EXT_DOWN_CNT, 188 data->link_stats.link_down_events)) 189 return -EMSGSIZE; 190 191 return 0; 192 } 193 194 const struct ethnl_request_ops ethnl_linkstate_request_ops = { 195 .request_cmd = ETHTOOL_MSG_LINKSTATE_GET, 196 .reply_cmd = ETHTOOL_MSG_LINKSTATE_GET_REPLY, 197 .hdr_attr = ETHTOOL_A_LINKSTATE_HEADER, 198 .req_info_size = sizeof(struct linkstate_req_info), 199 .reply_data_size = sizeof(struct linkstate_reply_data), 200 201 .prepare_data = linkstate_prepare_data, 202 .reply_size = linkstate_reply_size, 203 .fill_reply = linkstate_fill_reply, 204 }; 205