net/ethtool/linkstate.c

   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 };
  17
  18 #define LINKSTATE_REPDATA(__reply_base) \
  19         container_of(__reply_base, struct linkstate_reply_data, base)
  20
  21 static const struct nla_policy
  22 linkstate_get_policy[ETHTOOL_A_LINKSTATE_MAX + 1] = {
  23         [ETHTOOL_A_LINKSTATE_UNSPEC]            = { .type = NLA_REJECT },
  24         [ETHTOOL_A_LINKSTATE_HEADER]            = { .type = NLA_NESTED },
  25         [ETHTOOL_A_LINKSTATE_LINK]              = { .type = NLA_REJECT },
  26         [ETHTOOL_A_LINKSTATE_SQI]               = { .type = NLA_REJECT },
  27         [ETHTOOL_A_LINKSTATE_SQI_MAX]           = { .type = NLA_REJECT },
  28 };
  29
  30 static int linkstate_get_sqi(struct net_device *dev)
  31 {
  32         struct phy_device *phydev = dev->phydev;
  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
  42                 ret = phydev->drv->get_sqi(phydev);
  43         mutex_unlock(&phydev->lock);
  44
  45         return ret;
  46 }
  47
  48 static int linkstate_get_sqi_max(struct net_device *dev)
  49 {
  50         struct phy_device *phydev = dev->phydev;
  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
  60                 ret = phydev->drv->get_sqi_max(phydev);
  61         mutex_unlock(&phydev->lock);
  62
  63         return ret;
  64 }
  65
  66 static int linkstate_prepare_data(const struct ethnl_req_info *req_base,
  67                                   struct ethnl_reply_data *reply_base,
  68                                   struct genl_info *info)
  69 {
  70         struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
  71         struct net_device *dev = reply_base->dev;
  72         int ret;
  73
  74         ret = ethnl_ops_begin(dev);
  75         if (ret < 0)
  76                 return ret;
  77         data->link = __ethtool_get_link(dev);
  78
  79         ret = linkstate_get_sqi(dev);
  80         if (ret < 0 && ret != -EOPNOTSUPP)
  81                 return ret;
  82
  83         data->sqi = ret;
  84
  85         ret = linkstate_get_sqi_max(dev);
  86         if (ret < 0 && ret != -EOPNOTSUPP)
  87                 return ret;
  88
  89         data->sqi_max = ret;
  90
  91         ethnl_ops_complete(dev);
  92
  93         return 0;
  94 }
  95
  96 static int linkstate_reply_size(const struct ethnl_req_info *req_base,
  97                                 const struct ethnl_reply_data *reply_base)
  98 {
  99         struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
 100         int len;
 101
 102         len = nla_total_size(sizeof(u8)) /* LINKSTATE_LINK */
 103                 + 0;
 104
 105         if (data->sqi != -EOPNOTSUPP)
 106                 len += nla_total_size(sizeof(u32));
 107
 108         if (data->sqi_max != -EOPNOTSUPP)
 109                 len += nla_total_size(sizeof(u32));
 110
 111         return len;
 112 }
 113
 114 static int linkstate_fill_reply(struct sk_buff *skb,
 115                                 const struct ethnl_req_info *req_base,
 116                                 const struct ethnl_reply_data *reply_base)
 117 {
 118         struct linkstate_reply_data *data = LINKSTATE_REPDATA(reply_base);
 119
 120         if (data->link >= 0 &&
 121             nla_put_u8(skb, ETHTOOL_A_LINKSTATE_LINK, !!data->link))
 122                 return -EMSGSIZE;
 123
 124         if (data->sqi != -EOPNOTSUPP &&
 125             nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI, data->sqi))
 126                 return -EMSGSIZE;
 127
 128         if (data->sqi_max != -EOPNOTSUPP &&
 129             nla_put_u32(skb, ETHTOOL_A_LINKSTATE_SQI_MAX, data->sqi_max))
 130                 return -EMSGSIZE;
 131
 132         return 0;
 133 }
 134
 135 const struct ethnl_request_ops ethnl_linkstate_request_ops = {
 136         .request_cmd            = ETHTOOL_MSG_LINKSTATE_GET,
 137         .reply_cmd              = ETHTOOL_MSG_LINKSTATE_GET_REPLY,
 138         .hdr_attr               = ETHTOOL_A_LINKSTATE_HEADER,
 139         .max_attr               = ETHTOOL_A_LINKSTATE_MAX,
 140         .req_info_size          = sizeof(struct linkstate_req_info),
 141         .reply_data_size        = sizeof(struct linkstate_reply_data),
 142         .request_policy         = linkstate_get_policy,
 143
 144         .prepare_data           = linkstate_prepare_data,
 145         .reply_size             = linkstate_reply_size,
 146         .fill_reply             = linkstate_fill_reply,
 147 };