net/ethtool/ts.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2
   3 #include <linux/net_tstamp.h>
   4 #include <linux/phy.h>
   5
   6 #include "netlink.h"
   7 #include "common.h"
   8 #include "bitset.h"
   9
  10 struct ts_req_info {
  11         struct ethnl_req_info           base;
  12 };
  13
  14 struct ts_reply_data {
  15         struct ethnl_reply_data         base;
  16         enum timestamping_layer         ts_layer;
  17 };
  18
  19 #define TS_REPDATA(__reply_base) \
  20         container_of(__reply_base, struct ts_reply_data, base)
  21
  22 /* TS_GET */
  23 const struct nla_policy ethnl_ts_get_policy[] = {
  24         [ETHTOOL_A_TS_HEADER]           =
  25                 NLA_POLICY_NESTED(ethnl_header_policy),
  26 };
  27
  28 static int ts_prepare_data(const struct ethnl_req_info *req_base,
  29                            struct ethnl_reply_data *reply_base,
  30                            const struct genl_info *info)
  31 {
  32         struct ts_reply_data *data = TS_REPDATA(reply_base);
  33         struct net_device *dev = reply_base->dev;
  34         int ret;
  35
  36         ret = ethnl_ops_begin(dev);
  37         if (ret < 0)
  38                 return ret;
  39
  40         data->ts_layer = dev->ts_layer;
  41
  42         ethnl_ops_complete(dev);
  43
  44         return ret;
  45 }
  46
  47 static int ts_reply_size(const struct ethnl_req_info *req_base,
  48                          const struct ethnl_reply_data *reply_base)
  49 {
  50         return nla_total_size(sizeof(u32));
  51 }
  52
  53 static int ts_fill_reply(struct sk_buff *skb,
  54                          const struct ethnl_req_info *req_base,
  55                          const struct ethnl_reply_data *reply_base)
  56 {
  57         struct ts_reply_data *data = TS_REPDATA(reply_base);
  58
  59         return nla_put_u32(skb, ETHTOOL_A_TS_LAYER, data->ts_layer);
  60 }
  61
  62 /* TS_SET */
  63 const struct nla_policy ethnl_ts_set_policy[] = {
  64         [ETHTOOL_A_TS_HEADER]   = NLA_POLICY_NESTED(ethnl_header_policy),
  65         [ETHTOOL_A_TS_LAYER]    = NLA_POLICY_RANGE(NLA_U32, 0,
  66                                                    __TIMESTAMPING_COUNT - 1)
  67 };
  68
  69 static int ethnl_set_ts_validate(struct ethnl_req_info *req_info,
  70                                  struct genl_info *info)
  71 {
  72         struct nlattr **tb = info->attrs;
  73         const struct net_device_ops *ops = req_info->dev->netdev_ops;
  74
  75         if (!ops->ndo_hwtstamp_set)
  76                 return -EOPNOTSUPP;
  77
  78         if (!tb[ETHTOOL_A_TS_LAYER])
  79                 return 0;
  80
  81         return 1;
  82 }
  83
  84 static int ethnl_set_ts(struct ethnl_req_info *req_info, struct genl_info *info)
  85 {
  86         struct net_device *dev = req_info->dev;
  87         const struct ethtool_ops *ops = dev->ethtool_ops;
  88         struct kernel_hwtstamp_config config = {0};
  89         struct nlattr **tb = info->attrs;
  90         enum timestamping_layer ts_layer;
  91         bool mod = false;
  92         int ret;
  93
  94         ts_layer = dev->ts_layer;
  95         ethnl_update_u32(&ts_layer, tb[ETHTOOL_A_TS_LAYER], &mod);
  96
  97         if (!mod)
  98                 return 0;
  99
 100         if (ts_layer == SOFTWARE_TIMESTAMPING) {
 101                 struct ethtool_ts_info ts_info = {0};
 102
 103                 if (!ops->get_ts_info) {
 104                         NL_SET_ERR_MSG_ATTR(info->extack,
 105                                             tb[ETHTOOL_A_TS_LAYER],
 106                                             "this net device cannot support timestamping");
 107                         return -EINVAL;
 108                 }
 109
 110                 ops->get_ts_info(dev, &ts_info);
 111                 if ((ts_info.so_timestamping &
 112                     SOF_TIMESTAMPING_SOFTWARE_MASK) !=
 113                     SOF_TIMESTAMPING_SOFTWARE_MASK) {
 114                         NL_SET_ERR_MSG_ATTR(info->extack,
 115                                             tb[ETHTOOL_A_TS_LAYER],
 116                                             "this net device cannot support software timestamping");
 117                         return -EINVAL;
 118                 }
 119         } else if (ts_layer == MAC_TIMESTAMPING) {
 120                 struct ethtool_ts_info ts_info = {0};
 121
 122                 if (!ops->get_ts_info) {
 123                         NL_SET_ERR_MSG_ATTR(info->extack,
 124                                             tb[ETHTOOL_A_TS_LAYER],
 125                                             "this net device cannot support timestamping");
 126                         return -EINVAL;
 127                 }
 128
 129                 ops->get_ts_info(dev, &ts_info);
 130                 if ((ts_info.so_timestamping &
 131                     SOF_TIMESTAMPING_HARDWARE_MASK) !=
 132                     SOF_TIMESTAMPING_HARDWARE_MASK) {
 133                         NL_SET_ERR_MSG_ATTR(info->extack,
 134                                             tb[ETHTOOL_A_TS_LAYER],
 135                                             "this net device cannot support hardware timestamping");
 136                         return -EINVAL;
 137                 }
 138         } else if (ts_layer == PHY_TIMESTAMPING && !phy_has_tsinfo(dev->phydev)) {
 139                 NL_SET_ERR_MSG_ATTR(info->extack, tb[ETHTOOL_A_TS_LAYER],
 140                                     "this phy device cannot support timestamping");
 141                 return -EINVAL;
 142         }
 143
 144         /* Disable time stamping in the current layer. */
 145         if (netif_device_present(dev) &&
 146             (dev->ts_layer == PHY_TIMESTAMPING ||
 147             dev->ts_layer == MAC_TIMESTAMPING)) {
 148                 ret = dev_set_hwtstamp_phylib(dev, &config, info->extack);
 149                 if (ret < 0)
 150                         return ret;
 151         }
 152
 153         dev->ts_layer = ts_layer;
 154
 155         return 1;
 156 }
 157
 158 const struct ethnl_request_ops ethnl_ts_request_ops = {
 159         .request_cmd            = ETHTOOL_MSG_TS_GET,
 160         .reply_cmd              = ETHTOOL_MSG_TS_GET_REPLY,
 161         .hdr_attr               = ETHTOOL_A_TS_HEADER,
 162         .req_info_size          = sizeof(struct ts_req_info),
 163         .reply_data_size        = sizeof(struct ts_reply_data),
 164
 165         .prepare_data           = ts_prepare_data,
 166         .reply_size             = ts_reply_size,
 167         .fill_reply             = ts_fill_reply,
 168
 169         .set_validate           = ethnl_set_ts_validate,
 170         .set                    = ethnl_set_ts,
 171 };
 172
 173 /* TS_LIST_GET */
 174 struct ts_list_reply_data {
 175         struct ethnl_reply_data         base;
 176         enum timestamping_layer         ts_layer[__TIMESTAMPING_COUNT];
 177         u8                              num_ts;
 178 };
 179
 180 #define TS_LIST_REPDATA(__reply_base) \
 181         container_of(__reply_base, struct ts_list_reply_data, base)
 182
 183 static int ts_list_prepare_data(const struct ethnl_req_info *req_base,
 184                                 struct ethnl_reply_data *reply_base,
 185                                 const struct genl_info *info)
 186 {
 187         struct ts_list_reply_data *data = TS_LIST_REPDATA(reply_base);
 188         struct net_device *dev = reply_base->dev;
 189         const struct ethtool_ops *ops = dev->ethtool_ops;
 190         int ret, i = 0;
 191
 192         ret = ethnl_ops_begin(dev);
 193         if (ret < 0)
 194                 return ret;
 195
 196         if (phy_has_tsinfo(dev->phydev))
 197                 data->ts_layer[i++] = PHY_TIMESTAMPING;
 198         if (ops->get_ts_info) {
 199                 struct ethtool_ts_info ts_info = {0};
 200
 201                 ops->get_ts_info(dev, &ts_info);
 202                 if (ts_info.so_timestamping &
 203                     SOF_TIMESTAMPING_HARDWARE_MASK)
 204                         data->ts_layer[i++] = MAC_TIMESTAMPING;
 205
 206                 if (ts_info.so_timestamping &
 207                     SOF_TIMESTAMPING_SOFTWARE_MASK)
 208                         data->ts_layer[i++] = SOFTWARE_TIMESTAMPING;
 209         }
 210
 211         data->num_ts = i;
 212         ethnl_ops_complete(dev);
 213
 214         return ret;
 215 }
 216
 217 static int ts_list_reply_size(const struct ethnl_req_info *req_base,
 218                               const struct ethnl_reply_data *reply_base)
 219 {
 220         struct ts_list_reply_data *data = TS_LIST_REPDATA(reply_base);
 221
 222         return nla_total_size(sizeof(u32)) * data->num_ts;
 223 }
 224
 225 static int ts_list_fill_reply(struct sk_buff *skb,
 226                               const struct ethnl_req_info *req_base,
 227                          const struct ethnl_reply_data *reply_base)
 228 {
 229         struct ts_list_reply_data *data = TS_LIST_REPDATA(reply_base);
 230
 231         return nla_put(skb, ETHTOOL_A_TS_LIST_LAYER, sizeof(u32) * data->num_ts, data->ts_layer);
 232 }
 233
 234 const struct ethnl_request_ops ethnl_ts_list_request_ops = {
 235         .request_cmd            = ETHTOOL_MSG_TS_LIST_GET,
 236         .reply_cmd              = ETHTOOL_MSG_TS_LIST_GET_REPLY,
 237         .hdr_attr               = ETHTOOL_A_TS_HEADER,
 238         .req_info_size          = sizeof(struct ts_req_info),
 239         .reply_data_size        = sizeof(struct ts_list_reply_data),
 240
 241         .prepare_data           = ts_list_prepare_data,
 242         .reply_size             = ts_list_reply_size,
 243         .fill_reply             = ts_list_fill_reply,
 244 };